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ABSTRACT 


Autonomous  underwater  vehicle  navigation  is  a  complex  problem  of  state 
estimation.  Accurate  navigation  is  made  difficult  due  to  a  lack  of  reference  navigation 
aids  or  use  of  the  Global  Positioning  System  (GPS)  that  could  establish  the  vehicles 
position.  Accurate  navigation  is  critical  due  to  the  level  of  autonomy  and  range  of 
missions  and  environments  into  which  an  underwater  vehicle  may  be  deployed. 
Navigational  accuracy  depends  not  only  on  the  initialization  and  drift  errors  of  the  low 
cost  Inertial  Motion  Unit  (IMU)  gyros  and  the  speed  over  ground  sensor,  but  also  on  the 
performance  of  the  sensor  fusion  filter  used. 

This  thesis  will  present  the  method  by  which  an  Extended  Kalman  Filter  (EKF) 
was  tuned  after  installation  of  an  IMU  in  the  ARIES  Autonomous  Underwater  Vehicle. 
The  goal  of  installing  the  IMU,  analyzing  the  navigational  results  and  tuning  the  EKF 
was  to  achieve  navigational  accuracy  in  the  horizontal  plane  with  a  position  error  of  less 
than  one  percent  of  distance  traveled  when  compared  with  GPS.  The  research  consisted 
of  IMU  installation  and  software  modifications  within  the  vehicle  to  fully  realize  the 
design  goal.  Data  collection  and  analysis  was  conducted  through  field  experiments  and 
computer  simulation.  A  significant  result  of  this  work  was  development  of  a  pseudo- 
adaptive  algorithm  to  vary  the  measurement  noise  values  in  selected  channels  to  force  a 
desired  response  in  the  filter  and  improve  accuracy  and  precision  in  the  state  estimates. 
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I.  INTRODUCTION 


A.  BACKGROUND 

Autonomous  underwater  vehicle  (AUV)  navigation  is  a  complex  problem  of  state 
estimation.  Accurate  navigation  is  further  made  difficult  due  to  a  lack  of  reference 
navigation  aids  or  use  of  the  Global  Positioning  System  (GPS)  that  could  be  used  to 
establish  the  vehicles  position.  Potential  mission  requirements  necessitate  accurate 
navigation  due  to  the  level  of  autonomy  and  the  potential  range  of  missions  and 
environments  into  which  an  underwater  vehicle  may  be  deployed.  Navigational  accuracy 
depends  not  only  on  the  initialization  and  drift  errors  of  the  low  cost  Inertial  Motion  Unit 
(IMU)  gyros  and  the  speed  over  ground  sensor,  but  also  on  the  performance  of  the  sensor 
fusion  filter  used. 

An  IMU  helps  the  problem  of  state  estimation  by  providing  accurate  sensory 
inertial  inputs  that  can  be  used  along  with  a  model  of  the  vehicle  dynamics.  The  outputs 
of  the  IMU  take  the  form  of  linear  accelerations  and  angular  rates  that  can  then  be  input 
into  a  system  model  that  will  allow  for  estimation  of  the  vehicles  state,  in  particular  the 
vehicle  position  that  is  necessary  for  conduct  of  various  missions. 

This  thesis  will  present  the  method  by  which  an  Extended  Kalman  Filter  (EKF) 
was  tuned  after  installation  of  an  IMU  in  the  ARIES  AUV.  The  goal  of  installing  the 
IMU,  analyzing  the  navigational  results  and  tuning  the  EKF  was  to  achieve  navigational 
accuracy  in  the  horizontal  plane  with  a  position  error  of  less  than  one  percent  of  distance 
traveled  when  compared  with  GPS.  The  research  consisted  of  IMU  installation  and 
software  modifications  within  the  vehicle  to  fully  realize  the  design  goal.  Data  collection 
and  analysis  was  conducted  through  field  experiments  and  computer  simulation.  A 
significant  result  of  this  work  was  development  of  a  pseudo-adaptive  algorithm  to  vary 
the  measurement  noise  values  in  selected  channels  to  force  a  desired  response  in  the  filter 
and  improve  accuracy  and  precision  in  the  state  estimates. 
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B.  MOTIVATION 

Unmanned  Underwater  Vehicles  (UUVs)  are  being  actively  pursued  in  the  United 
States  Navy  as  a  means  to  enhance  war  fighting  in  the  underwater  realm.  UUVs  refer  to 
both  remotely  controlled  vehicles  and  AUVs.  These  vehicles  can  be  used  in  a  wide  range 
of  mission  functions  from  Intelligence,  Surveillance,  and  Reconnaissance  to  Mine 
Warfare  to  Salvage  and  Recovery  operations  as  detailed  in  the  Navy’s  Master  UUV  Plan 
(2004).  They  are  a  force  multiplier  and  allow  the  Navy  the  accomplish  missions  that  may 
be  too  dangerous  or  impractical  for  current  practices.  “The  long-tenn  UUV  vision  is  to 
have  the  capability  to:  (1)  deploy  or  retrieve  devices,  (2)  gather,  transmit,  or  act  on  all 
types  of  information,  and  (3)  engage  bottom,  volume,  surface,  air  or  land  targets  (UUV 
Master  Plan,  2004).”  Figure  1  shows  UUVs  in  use  during  recent  military  operations. 
These  missions  require  accurate  navigation  to  perform  their  tasks. 

UUVs  at  War:  Operation  Iraqi  Freedom 


Figure  1.  Tactical  Application  of  AUV  (From:  UUV  Master  Plan,  2004) 


The  motivation  for  this  thesis  research  was  to  obtain  navigational  estimates  of 
position  in  the  horizontal  plane  that  were  within  one  percent  error  of  the  distance 
traveled.  This  design  goal  would  need  to  be  addressed  through  both  software  and 
hardware  configuration  changes  to  the  navigation  architecture  of  the  ARIES  vehicle.  The 
most  significant  physical  change  was  in  the  hardware  configuration  which  would  utilize  a 
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relatively  low  cost  IMU  currently  used  in  production  of  military  missile  technology. 
Once  the  gains  in  positional  accuracy  from  the  hardware  were  realized  the  navigation 
fdter  would  be  tuned  in  order  to  achieve  the  design  goal. 

C.  THE  ARIES  VEHICLE 

“The  ARIES  is  used  for  development  of  computer  architecture,  software,  sensors 
and  navigational  hardware  for  small  to  medium  sized  autonomous  systems  (Healey, 
2006).”  It  is  approximately  three  meters  in  length  and  is  fitted  with  various  sensors  and 
electronics  in  order  to  carry  out  the  development  and  research  noted  above.  Figure  2 
shows  the  ARIES  being  loaded  onto  the  research  support  vessel  Cyprus  Sea  in  Monterey 
Harbor. 


Figure  2.  ARIES  Operations  in  Monterey  Bay  (From:  Healey,  2006) 


The  vehicles  nominal  operating  speed  is  1.2  to  1.5  meters  per  second  developed 
from  twin  thrusters  mounted  in  the  rear.  The  vehicle  operates  off  of  a  bank  of  batteries 
that  provide  a  nominal  bus  voltage  of  60  volts  for  the  vehicle  propulsion  and  hotel  loads. 
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Vertical  maneuvering  control  for  the  vehicle  is  provided  by  two  forward  and  two  rear 
plane  surfaces.  Steering  control  is  provided  by  a  top  mounted  twin  rudder  configuration, 
one  forward  and  one  aft. 

Communications  with  the  vessel  occur  through  several  antennas  on  top  of  the 
vehicle  which  include  802.11  type  wireless  digital  communication  as  well  as  standard 
radio  free  wave  communication.  A  GPS  receiver  is  mounted  on  top  of  the  aft  rudder  to 
allow  for  receipt  of  GPS  signals  when  on  the  surface.  Navigation  in  the  ARIES  is 
performed  without  the  use  of  any  radio  beacons  and  relies  solely  on  GPS,  inertial 
navigation  system,  and  the  Doppler  speed  sensor. 

There  are  three  main  compartments  in  the  vehicle  that  house  the  equipment 
necessary  to  run  the  vehicle.  The  forward  compartment  consists  of  a  PC- 104  computer 
that  operates  the  sonar  imaging  obtained  from  ARIES  forward  looking  blazed  array 
sonar.  The  forward  compartment  also  houses  the  servos  for  the  forward  control  surfaces. 
The  mid  compartment  is  the  largest  and  houses  the  vehicle  relays  for  ancillary  sensors, 
the  battery  banks,  and  the  two  main  computers  used  for  operating  the  vehicle.  There  is 
one  computer  dedicated  to  executive  level  process  management  and  mission  execution 
and  one  computer  dedicated  to  tactical  execution  of  commands  used  for  vehicle  motion. 
Finally,  the  aft  compartment  houses  the  motors  for  the  thrusters  and  servos  for  the  aft 
control  surfaces,  the  wireless  router  for  communications  and  on  the  forward  bulkhead  of 
the  aft  compartment,  the  IMU. 

D.  THESIS  STRUCTURE 

The  research  conducted  was  an  attempt  to  achieve  navigational  errors  within  one 
percent  of  the  distance  traveled.  This  was  an  iterative  approach  conducted  over  an 
approximately  one  year  period  that  involved  data  analysis  from  field  experiments  coupled 
with  computer  simulation.  The  analyses  of  the  errors  and  modifications  made  to  the 
vehicle  from  each  significant  evolution  are  presented. 

Chapter  II  will  present  the  theory  of  the  inertial  navigation  model  used,  covering 
specifically  the  operation  of  inertial  motion  units  and  the  theory  of  the  EKF  as  used  in  the 
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ARIES  vehicle.  Chapter  III  will  provide  the  details  of  the  IMU  installation  into  the 
vehicle.  Chapter  IV  will  detail  the  software  changes  that  were  implemented  over  the 
course  of  this  work.  Chapter  V  will  present  the  field  experiments  and  the  geometries 
utilized,  and  the  results  obtained,  presenting  in  detail  the  navigational  errors.  Finally, 
Chapter  VI  provides  thesis  conclusions  and  recommendations  for  future  work.  The 
supporting  code  utilized  in  this  work  will  be  retained  in  the  appendices  to  this  thesis. 
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II.  THEORY  OF  INERTIAL  NAVIGATION 


A.  INTRODUCTION 

Inertial  navigation  systems  (INS)  have  become  increasingly  widespread  in  their 
use  over  the  last  few  decades.  This  is  due  in  large  part  to  technological  gains  made  in 
computing  power  and  in  increasing  sensitivity  of  inertial  sensor  units  made  from  quality 
manufacturing.  The  backbone  of  an  inertial  navigation  system  is  the  IMU,  which  has 
gained  increasing  use  due  to  manufacturing  techniques  allowing  the  production  of  small 
and  accurate  light  weight  systems  as  well  as  larger  extremely  precise  units. 

Coupled  with  the  several  order  of  magnitude  increase  in  computing  power  that 
has  been  observed  in  the  last  few  decades  is  the  development  of  sophisticated  algorithms 
that  can  process  and  manipulate  the  large  data  streams  from  an  inertial  sensor.  These 
algorithms  allow  for  increased  accuracy  in  state  estimation  by  using  more  information 
and  increasingly  smaller  time  steps  in  computing  dynamic  information.  The  latter  is  a 
result  of  extremely  fast  processing  speeds  now  capable  in  computer  systems.  These 
algorithms  allow  for  inertial  navigation  of  vehicles  relying  on  inputs  from  inertial  sensors 
for  navigation  such  as  underwater  vehicles  and  submarines  without  continuous  external 
reference  navigation  inputs. 

B.  INERTIAL  MOTION  UNIT 

Inertial  motion  units  (also  called  inertial  measurement  units)  have  become 
increasingly  popular  as  measurement  devices  of  vehicle  motion  for  use  in  inertial 
navigation  systems.  These  units  come  in  a  wide  variety  of  forms  from  simple  strap-down 
systems  to  extremely  accurate  complex  gimbaled  or  stabilized  platfonn  systems.  These 
instruments  work  by  sensing  the  vehicle’s  inertial  linear  accelerations  and  angular 
rotation  rates  which  can  then  be  sent  to  a  computer  for  processing  with  a  filter  that  fuses 
data  from  different  sensors.  The  intricate  details  of  inertial  motion  units  are  well 
published  and  the  following  provides  a  basic  understanding  of  the  strap-down  style 
system  that  was  used  for  this  research. 
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A  strap-down  style  IMU  is  ideal  for  a  wide  range  of  applications;  particularly  in 
vehicles  where  space  is  limited  due  to  its  relatively  small  size  and  low  weight.  These 
units  are  robust  and  their  use  of  solid  state  electronics  make  them  more  reliable  than 
mechanical  gimbaled  systems  (Yakimenko,  2006).  For  the  details  of  mathematics  and 
mechanization  of  a  strap-down  IMU  the  reader  is  referred  to  Siouris  (1993).  The  strap- 
down  system  uses  accelerometers  to  measure  linear  accelerations  and  a  set  of  gyroscopes 
to  measure  the  angular  rates  of  the  body.  From  these  accelerations  velocity  and  position 
information  may  be  obtained  through  integration.  The  gyroscopes  measure  the  rate  at 
which  the  vehicles  attitude  changes.  From  this  data  the  vehicles  yaw,  pitch,  and  roll  may 
be  obtained.  A  simple  illustration  of  a  strap-down  IMU  is  shown  in  Figure  3. 


Figure  3.  Basic  Strap-down  IMU  (From:  Roth,  1999) 


C.  EXTENDED  KALMAN  FILTER 

The  EKF  was  developed  to  address  the  problem  of  utilizing  a  standard  Kalman 
filter,  a  linear  estimator,  for  problems  involving  non-linear  system  dynamics  and 
measurements.  Motion  of  vehicles  with  non-linear  dynamics  can  be  made  locally  linear 
through  the  use  of  the  Jacobian  of  the  dynamics  matrix  from  the  vehicle  equations  of 
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motion.  By  taking  a  first-order  series  expansion  of  the  non-linear  equations,  the  system  is 
linearized  about  a  point  in  time  (Bar-Shalom,  2001).  The  algorithm  can  then  employ  the 
estimation  techniques  of  a  Kalman  filter  to  produce  an  estimate  of  the  vehicle  state  at  an 
instant  in  time. 

1.  Assumptions  of  the  EKF 

The  EKF  utilizes  some  well  known  assumptions  that  are  an  extension  from  the 
original  Kalman  Filter.  These  assumptions  are  relevant  when  comparing  filters  of 
various  types,  e.g.  the  Unscented  Kalman  Filter,  in  order  to  understand  the  strengths  and 
weaknesses  of  different  estimation  methods.  The  EKF  assumes  that  the  process  noise 
(q(t))  and  the  measurement  noise  (v(t))  are  white,  additive  and  zero  mean.  Additionally, 
the  process  and  measurement  noises  are  uncorrelated,  i.e.  independent  of  each  other. 
Finally,  the  initial  state  and  its  covariance  are  independent  of  either  the  process  or 
measurement  noise.  In  summary  the  process  and  measurement  noise  are  given  as  (Bar- 
Shalom,  2001): 


e  (q(t))  =  o 

E(qq)  =  Q 

(1) 

£(v(t))  =  0 

E(W)  =  R 

(2) 

2.  System  Model 

The  literature  on  EKFs  is  well  developed  and  extensive;  therefore,  the  following 
discussion  will  focus  on  the  specifics  of  the  EKF  as  used  in  the  ARIES  AUV.  For  more 
information  on  EKFs  the  reader  is  referred  to  Bar-Shalom  (2001). 

Navigational  state  estimates  in  the  ARIES  vehicle  are  made  by  utilizing  an  EKF 
to  predict  and  correct  these  states  every  0.125  seconds.  The  state  of  the  vehicle  for  the 
horizontal  plane  is  contained  in  an  eight  state  vector  as  follows  (Healey,  1995): 

x(0  =  [X,  Y,y/,r,ug,vg,br,bv ] '  (3) 
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Where: 


X  =  North-South  position  in  Local  Navigation  Plane 

Y  =  East-West  position  in  Local  Navigation  Plane 

*P  =  Heading 

r  =  Yaw  (Heading)  rate 

ug  =  Forward  speed  over  ground 

vg  =  Lateral  speed  over  ground 

br  =  Yaw  rate  bias 

bv  =  Heading  bias 


The  system  dynamics  and  measurement  model  are  given  by  (Healey,  1995): 

x(t)  =  f(x(t))  +  q(t) 
y(t)  =  h(x(t))  +  v(t) 


For  ARIES  the  vector  valued  function  h(x(t))  is  constant  and  can  be  represented 
as: 


y(t)  =  Cx(t) 


(5) 


The  EKF  may  be  formulated  in  either  continuous  or  discrete  time;  the  discrete 
time  is  necessary  for  computer  application  (Healey,  1995).  The  vehicle  states  are  linked 
through  the  following  vehicle  equations  of  motion  and  are  contained  in  the  dynamics 
matrix. 
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X  =  ug  cos (y/)-v  sin(^) 
Y  =  ugsin(i//)  +  vgcos(i//) 
if/  —  r 
r  =  0 

“*=0 

*.=° 

*,=  o 

*r=° 


(6) 


Linearizing  these  equations  yields  the  transition  matrix,  <t>,  used  to  propagate  the 
state  and  covariance  matrix  between  time  steps  in  the  discrete  model. 

The  measurements  are  associated  with  the  state  vector  as  follows: 


Vi  =  ug, 

y2  =  v 

v3  =yf  +  bv\ 

y  4  =  f  +  br; 
v5  =  X; 
y6  =  Y; 

or 

y  =  h(x(t))  =  Cx(t) 


The  discrete-time  filter  can  then  be  represented  as: 


Xi+l/i 

P 

i+l/i 


-  <H|i 

=  a>pin<t>+Q 


L,+i/,=Pi+i/,C'(CPi+1|iC'  +  R y1 


Xi+l|i+l 

P 

x  i+l|i+l 


=  xi+1/i+Li+1(yi+1-Cxi+1/i) 
=  (I-Li+1C)Pi+1|i 


Where: 

x  =  Estimate  of  state 
P  =  Covariance  of  states 
Q  =  Process  noise  matrix 
R  =  Measurement  noise  matrix 


(V) 


(8) 
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L  =  Kalman  Gain 


The  resulting  algorithm  provides  discrete  state  updates  at  each  time  step  and 
predicts  the  successive  time  step  and  then  corrects  the  estimate  based  on  the 
measurements  received.  When  properly  tuned  and  with  good  quality  sensors  the  EKF 
provides  very  good  estimates  of  the  vehicle  state.  Tuning  the  filter  is  done  through  the 
choice  of  the  values  for  the  process  and  measurement  noise  matrices,  based  on  known 
information  about  the  sensors  and/or  experience  through  application  of  the  filter. 

D.  NAVIGATION  ERROR  DEFINED 

The  navigational  accuracy  must  be  defined  to  provide  a  standard  quantitative 
measure  by  which  to  evaluate  the  changes  made  to  the  ARIES  inertial  navigation  system. 
For  this  work,  only  motion  in  the  horizontal  plane  was  analyzed.  The  absolute  error  (E ) 
in  meters  is  defined  as  norm  of  the  error  vector  taken  as  the  difference  between  the  initial 
GPS  position  at  a  time  step  (/)  and  the  navigation  filter  estimate  at  the  (i-1)  time  step. 


£=V(X< 


LGra<o 


-X 


NavFilter(i-l) 


r+h 


GPS(i) 


-Y, 


NavFilter(i-l) 


(9) 


The  method  in  which  the  GPS  information  is  used  when  obtained  in  the 
navigation  process  required  the  use  of  the  (i-1)  position.  This  is  because  at  time  step  (i), 
if  GPS  position  information  is  available  this  information  is  inserted  into  the  EKF  for  the 
X  and  Y  measurements  and  the  estimates  are  updated,  providing  a  new  position  estimate 
based  on  this  information.  It  was  necessary  to  see  where  the  filter  thought  it  was  located 
prior  to  obtaining  updated  position  information  compared  to  the  actual  vehicle  position 
determined  by  GPS.  The  assumption  was  made  that  GPS  information  was  absolute  truth 
for  position  in  order  to  compare  filter  performance.  The  processing  of  GPS  information 
had  to  be  modified  slightly  in  order  to  obtain  increased  accuracy  by  rejecting  fixes  for 
which  the  number  of  satellites  visible  to  the  receiver  were  below  a  set  threshold.  This 
modification  is  discussed  further  in  Chapter  IV.  The  error  induced  from  using  the  (i-1) 
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position  is  at  most  vehicle  speed  times  the  time  interval  of  0.125  seconds,  or  0.15  meters, 
which  can  be  considered  negligible  in  this  analysis. 

E.  THEORETICAL  ERROR  ANALYSIS 

The  ideal  position  error  can  be  estimated  from  the  manufacturers  published  gyro 
drift  rate  for  the  IMU  and  some  basic  assumptions  for  vehicle  dynamics.  The  Honeywell 
HG1700  has  a  published  drift  rate  of  one  degree  per  hour.  Given  the  dynamics  as: 


Y  =  W¥  (10) 

(11) 

Where: 

Y  =  Cross  Track  Error  Rate 
'F  =  Heading  Error 

'f<0=Heading  Initialization  Error  =  1  deg 
k  =  Gyro  Drift  Rate  =  1  deg/  hr 
U  =  Forward  Vehicle  Speed  =  1 2m  /  5 


Y  =  Jr  V  dt  =\TQU(kt  +  ^0)dt 


Y  =  U 


(  rr^‘  ^ 

V  2  J 


(12) 


The  theoretical  cross  track  error  for  a  thirty  minute  run  is  as  follows: 


f  =  1.2— 

sec 


deg  1  rad  0.52  hr2  ,  ,  1  rad 


hr  57.3 deg  2 


-  + 1  deg- 


57.3  deg 


•0.5  hr 


A  A 3600 sec ^ 


) 


V  lhr  7 


=  47.1m 


Y  =  Distance  Traveled  =  1 .2  —  •  1 800sec=2 1 60  m 

sec 


The  resulting  error  as  a  percentage  of  distance  traveled  is  2.18%.  Based  on  this  a 


design  goal  of  one  percent  of  the  distance  traveled  was  set  for  this  research.  Figure  4 
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illustrates  the  theoretical  cross  track  error  that  develops  over  time  based  on  the  above 
formulations.  Figure  5  illustrates  the  percentage  error  growth  over  time.  Both  figures  are 
parameterized  by  the  initialization  error  from  using  the  original  compass  to  align  the 
IMU.  The  effects  of  the  initialization  error  can  be  seen  on  the  resulting  cross  track  error 
and  error  per  unit  distance  traveled. 


Figure  4.  Theoretical  Cross  Track  Error 
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Figure  5.  Theoretical  Error  Growth 


As  can  be  seen  from  these  two  figures  the  amount  of  error  in  position  estimates 
that  will  develop  is  strongly  dependent  on  the  initialization  error  of  the  IMU  and  to  some 
degree  the  drift  rate  of  the  gyro.  Therefore,  it  is  critical  that  the  navigation  filter  learn  the 
heading  bias  quickly  in  order  to  compensate  for  this  initialization  error.  The  rate  of 
learning  must  be  balanced  against  the  overall  performance  of  the  filter. 
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III.  HARDWARE  IMPLEMENTATION 


A.  INTRODUCTION 

The  ARIES  vehicle  originally  used  a  compass  to  provide  heading  and  a  Systran 
Donner  Motion  Pak  IMU  provided  gyro  rate  inputs  to  the  navigation  filter.  The  compass 
was  a  Honeywell  HMR3000  magneto-restrictive  compass  and  provided  reasonable 
outputs  for  the  sensor  (Marco  et  ah,  2001).  However,  a  compass  has  the  disadvantage  of 
induced  errors  from  the  true  heading  due  to  magnetic  field  variations  caused  locally 
around  and  within  the  vehicle.  Additionally,  there  will  be  a  deviation  from  true  North 
based  on  location  of  operations  that  must  be  corrected  for.  As  a  result  the  compass  would 
lose  accuracy  over  time  and  produce  significant  navigational  errors. 

The  accuracy  achievable  with  the  compass  is  a  direct  result  of  the  ability  to 
conduct  calibrations  with  the  compass  and  vehicle  as  well  as  a  reasonably  accurate  and 
updated  deviation  table  used  for  corrections.  Another  difficulty  in  utilizing  the  compass 
was  that  the  heading  bias  learned  was  dependent  on  heading  and  thus  required  extensive 
use  of  the  deviation  tables  (Kragelund,  2006).  The  calibrations,  if  successful, 
significantly  reduced  the  error  that  resulted  in  the  navigation  filter  but  they  were  difficult 
to  obtain  good  runs  and  required  a  considerable  amount  of  time. 

B.  INERTIAL  MOTION  UNIT  INSTALLATION 

For  this  work  a  Honeywell  HG1700  IMU  was  purchased  and  installed  in  the 
vehicle.  The  HG1700  is  a  strap-down  type  of  IMU  and  is  considered  a  low  cost  military 
grade  production  unit.  The  location  was  selected  to  place  the  unit  as  close  to  body  center 
as  possible  without  displacing  elements  of  the  vehicle  previously  installed.  The  inertial 
motion  unit  and  associated  electronic  circuitry  represent  the  only  hardware  modification 
associated  with  this  work. 

The  IMU  was  installed  in  the  aft  compartment  mounted  vertically  against  the 
forward  bulkhead.  Figure  6  illustrates  the  installation  of  this  unit. 
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Figure  6.  IMU  Installed  in  ARIES 


The  unit  was  centered  on  the  body  lateral  and  vertical  axis  and  mounted  in  place 
for  the  output  values  to  correspond  directly  to  the  body  local  x,  y  and  z  axis.  The  body 
defined  axis’  for  the  ARIES  uses  traditional  orientations  with  the  x-axis  along  the  body 
centerline,  the  y-axis  is  athwartships  on  the  body  and  the  z  axis  is  defined  positive  in  the 
downward  direction. 

The  unit  utilizes  +/-  15  volt  DC  and  +5  volt  DC  input  power  for  the  associated 
electronics.  This  power  is  tapped  off  of  the  main  battery  bus  power  and  stepped  down 
from  60  volts  to  the  required  input  power  using  two  Calex  DC-DC  converters  shown  in 
Figure  6.  The  larger  one  supplies  the  +/-  15  volt  DC  power  while  the  smaller  supplies  the 
+5  volt  DC  power  (Kragelund,  2006). 
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IV.  SOFTWARE  IMPLEMENTATION 


A.  INTRODUCTION 

Once  the  hardware  was  installed  and  the  results  of  the  hardware  were  analyzed, 
attention  was  turned  towards  the  software  architecture  of  the  navigation  filter  realizing 
that  to  achieve  the  design  goal  of  one  percent  of  distance  traveled  both  hardware  and 
software  improvements  were  going  to  be  required.  The  approach  taken  was  dual  in 
nature  utilizing  simulation  and  field  experiments.  The  EKF  presented  in  Chapter  II  is 
embedded  in  the  navigation  code  and  is  often  referred  to  as  the  navigation  filter.  The 
tenns  EKF  and  navigation  filter  are  synonymous  with  respect  to  this  work. 

This  chapter  will  present  the  changes  made  to  the  system  software  as  well  as  what 
was  in  use  for  the  operating  system.  Additionally,  the  method  in  which  MATLAB®  was 
used  to  enhance  the  result  of  this  research  for  the  UKF  and  EKF  will  be  shown.  The 
order  of  the  following  sections  represents  modifications  as  performed  in  chronological 
order.  The  results  obtained  from  these  changes  are  shown  in  Chapter  V  in  the  same 
chronological  order 

1.  Vehicle  Operating  System 

The  ARIES  vehicle  utilizes  a  QNX  real  time  operating  system  with  code  written 
in  ANSI  -  C  for  both  the  tactical  and  executive  computers.  The  system  operates  on  an  8 
hertz  (Hz)  data  cycle  and  the  measurements  obtained  for  state  infonnation  are 
asynchronous.  GPS  data,  when  available,  have  a  frequency  of  1  Hz.  Speed  information 
from  the  RDI  Doppler  is  provided  at  a  frequency  of  2  Hz.  Lastly,  the  IMU  provides  data 
at  100  Hz  but  is  selectively  chosen  over  each  8  Hz  cycle.  This  will  be  discussed  more  in 
section  B. 

2.  MATLAB®  Simulation 

For  simulations,  a  MATLAB®  code  was  developed  based  on  previous  work  by  A. 
Healey,  which  modeled  the  navigation  filter  in  the  vehicle  and  utilized  data  obtained  from 
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experiments.  This  code  was  then  manipulated  to  determine  effects  on  navigation 
performance  from  changes  to  the  filter.  The  MATLAB®  version  of  this  code  is 
contained  in  Appendix  B. 

3.  Unscented  Kalman  Filter 

In  recent  years  there  have  been  many  attempts  in  the  literature  to  improve  upon 
the  EKF  due  to  the  difficulties  in  implementation  and  tuning.  The  EKF  requires 
reasonable  determinations  of  the  process  and  measurement  noise  for  the  system  in  order 
to  implement;  this  can  be  rather  difficult  and  may  require  much  time  to  achieve  optimal 
performance. 

The  Unscented  Kalman  Filter  (UKF)  was  developed  by  Simon  Julier  and  Jeffrey 
Uhlmann  (1997)  as  an  alternative  method  to  the  EKF.  This  filter  characterizes  the  mean 
and  covariance  parameters  by  the  use  of  a  set  of  discrete  points  (Julier  et.  al,  1997). 
These  points  are  then  propagated  through  time  and  the  mean  and  covariance  are 
reconstructed  to  provide  the  updated  estimate  of  the  state.  This  filter  does  not  require  that 
the  process  or  measurement  noise  be  Gaussian  and  the  Jacobian  is  not  required  to  be 
calculated  for  the  system.  These  last  two  are  the  major  advantages  for  this  type  of  filter 
over  the  EKF.  The  literature  indicates  that  this  particular  application  of  Kalman  Filter 
has  shown  better  results  for  particular  applications  when  compared  to  the  EKF.  Based  on 
this  latter  claim  a  UKF  was  attempted  for  implementation  in  the  navigation  filter  of  the 
ARIES  AUV. 

A  UKF,  based  on  Julier  and  Uhlmann’s  work  was  developed,  in  MATLAB®  for 
ARIES  navigation  by  Dr.  Hag  Seong  Kim,  a  research  assistant  with  the  AUV  Research 
Group  assisting  with  this  work,  utilizing  data  from  experiments.  The  MATLAB® 
version  of  this  filter  is  contained  in  Appendix  C.  This  filter  was  run  for  a  multitude  of 
values  for  process  and  measurement  noise  and  compared  to  the  in-vehicle  filter.  Figure  7 
shows  the  results  of  the  UKF  developed  when  compared  to  the  EKF  contained  in  the 
vehicle. 
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Figure  7.  UKF  vs.  EKF  for  ARIES  Run  -  9/7/05 


It  was  detennined  that,  for  this  vehicle  and  application,  the  UKF  produced  results 
and  errors  on  the  same  scale  as  the  EKF  without  sufficient  improvement  in  accuracy.  It 
was  noted  that  the  UKF  provided  tighter  estimates  of  position  when  updated  with  GPS 
and  the  same  results  can  be  observed  with  the  speed  components  where  less  filtering 
occurs  and  more  of  the  dynamics  are  retained  in  the  estimates.  However,  when  later 
work  was  performed  with  tuning  the  vehicle  EKF,  better  results  were  obtained. 
Therefore,  further  work  in  code  development  and  implementation  in  the  vehicle  was  not 
warranted  for  the  UKF.  It  can  be  said  that  the  implementation  of  the  filter  was  slightly 
easier  since  derivatives  were  not  necessary,  however,  the  same  difficulties,  i.e., 
determining  appropriate  process  and  measurement  noise  matrices,  encountered  in  tuning 
the  EKF,  would  be  the  same  for  the  UKF. 


4.  Vehicle  Code 

There  were  two  primary  areas  of  vehicle  code  that  were  both  developed  and 

modified  in  order  to  install  and  make  usable  the  data  from  the  IMU.  The  first  was  an 
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operating  code  for  the  IMU  that  would  control  and  convert  the  data  flow  from  the  IMU  to 
be  used  by  the  navigation  filter  in  the  onboard  computers.  The  original  IMU  code  was 
developed  by  Jack  Nicholson,  CAPT,  USN  during  the  installation  of  the  IMU.  This  code 
is  contained  in  Appendix  D.  The  second  was  the  navigation  filter  code  which  contained 
the  routines  of  the  EKF  that  developed  vehicle  state  information  at  each  time  step.  A 
copy  of  this  code  is  contained  in  Appendix  E. 

The  ARIES  operating  system  operates  asynchronously  accepting  sensor  inputs 
from  multiple  locations  at  different  frequencies.  The  vehicle  operates  at  a  high  enough 
time  constant  to  be  able  to  ensure  no  aliasing  occurs  among  the  sensors  and  the  data.  The 
vehicle  time  constant  can  be  estimated  to  be: 


L  3  m 

t  =  —  □ - =  2  sec 

U  1 .5m  /  s 

This  leads  to  a  dynamic  frequency  for  the  vehicle  of: 


f  = 

J  max 


1 


2  sec 


=  0.5  Hz 


Therefore  according  to  the  Nyquist  Sampling  Criterion: 

f sample  ^  /max  ^  EC 


(13) 


(14) 


(15) 


The  conclusion  here  is  that  as  long  as  the  sensor  inputs  occur  at  a  rate  greater  than 
1  Hz,  the  vehicle  dynamics  are  slow  enough  to  ensure  that  adequate  sampling  of  those 
dynamics  by  the  sensors  are  sufficient.  This  is  important  from  the  standpoint  of  utilizing 
IMU  data  to  be  discussed  shortly. 


B.  INERTIAL  MOTION  UNIT  CODE 

The  IMU  code  used  by  the  ARIES  was  developed  to  handle  the  flow  of  data 
obtained  from  the  Honeywell  HG1700.  The  IMU  provides  inertial  information  for  linear 
acceleration  and  angular  rotation  rates  in  three  dimensions  for  the  vehicle  in  the  local 
body  fixed  frame.  The  code  converted  the  readings  into  the  local  navigational  tangent 
plane  and  corrected  the  measurements  for  the  Earth’s  rotational  rate.  While  three 
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dimensional  inertial  infonnation  can  be  obtained  from  the  unit,  it  was  the  heading  rate 
that  was  of  primary  concern  for  the  navigation  filter.  The  heading  rate  was  integrated 
within  the  code  using  a  simple  Euler  integration  scheme  over  the  8  Hz  time  interval.  The 
simple  integration  scheme  was  justified  based  on  the  short  time  interval  compared  with 
the  rate  of  vehicle  motion.  The  result  of  this  integration  was  a  sufficiently  accurate 
heading  input  to  replace  the  compass  heading  previously  utilized. 

The  IMU  code  provides  for  the  necessary  rotations  and  adjustments  that  are 
necessary  to  obtain  truly  inertial  measurements  from  the  IMU.  The  accelerometers  and 
gyros  associated  with  the  IMU  measure  the  acceleration  and  rotations  felt  upon  the 
sensor,  and  not  all  of  the  measurement  is  due  to  body  motion  of  the  vehicle.  The  IMU 
will  sense  the  rotation  of  the  Earth  frame  in  which  the  vehicle  is  moving.  The 
measurements  thus  obtained  from  the  IMU  must  be  corrected  for  the  Earth’s  sidereal 
rotation  rate.  Once  the  IMU  information  has  been  rotated  and  corrected  it  can  then  be 
fused  with  other  data  obtained  in  the  same  reference  plane  with  which  position  estimates 
can  then  be  made. 

One  of  the  primary  calculations  needed  is  to  rotate  the  Earths  angular  rotation  rate 
of  approximately  15.04  degrees  per  hour  into  the  body  fixed  frame.  This  is  necessary  to 
obtain  purely  inertial  measurements  for  the  vehicle  since  the  local  navigation  plane  is 
rotating.  Figure  8  provides  the  illustration  showing  the  relationship  between  the 
navigation  plane  the  vehicle  moves  in  with  the  Earth’s  rotating  reference  frame.  The 
local  navigation  plane  oriented  in  the  North-East-Down  configuration  rotates  at  the 
Earths  sidereal  rotate  and  this  rotation  is  felt  upon  the  gyros  in  the  IMU. 


23 


x.,  yv  zi  or  x,  y,  z  =  inertial 

xg,  yg,  zg  =  earth-fixed 

N,  E,  D  or  xn,  yn,  zn  =  geographic  (or  navigational  frame) 

xc’  yc>  zc  “  wander-azimuth  (or  computational  frame) 

Figure  8.  Earth  Frame  of  Reference  (From:  Yakimenko,  2006) 


Once  rotated,  the  elements  corresponding  to  the  Earth’s  rotation  can  be  removed 
from  the  measurements  for  angular  rates  within  the  IMU.  Only  those  elements  needed  to 
obtain  the  heading  rate  were  calculated.  The  equations  and  process  used  are  outlined  as 
follows  and  are  contained  in  the  IMU  code  in  Appendix  D. 

1)  The  Earth’s  sidereal  rotation  vector  [0,0,0]’  is  rotated  into  the  body  fixed  frame 
[pe,qe,re]’  using  the  current  geodetic  latitude  of  the  vehicle  and  the  vehicle  body 
Euler  angles  ((p,0,v|/). 
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qe  =  [-cos(^)-sin(^)-cos(Lat)-sin(^)-cos(#)-sin(Lat)]-Q  (16) 

re  =  [(sin( (/i)  •  sin(  ^)+cos(^)  •  sin( 0)  ■  cos(  y/))  ■  cos(Lat)-cos(^)  •  cos( 0)  •  sin(Lat)]  •  Q  (17) 


2)  The  elements  of  the  Earths  sidereal  rotation  rate  are  then  removed  from  the 
angular  rate  measurements  from  the  IMU  gyros  [pout,  qout,  rout]’. 


Qout  Qout 


r  =  r  —  r 

out  out  e 


(18) 


3)  The  adjusted  angular  rate  values  which  represent  the  true  angular  rate  of  the 
vehicle  with  respect  to  a  true  inertial  frame  can  be  used  to  obtained  the  heading 
rate  ( \j/  ). 


sin(^)  cos(^) 

co%(d)'q°u,  +  co^(d)r°ut 


(19) 


4)  The  heading  rate  is  then  integrated  using  Euler  integration  with  the  result  being 
an  updated  heading  value  at  the  i+1  time  step. 


<1 

II 

<1 

(20) 

Vm  =Yi+ky 

(21) 

The  output  of  the  IMU  is  at  100  Hz,  i.e.  there  are  100  readings  of  all  channels  per 
second.  Originally,  twelve  readings  were  averaged  over  an  8  Hz  period  in  order  to  align 
the  IMU  output  with  the  operating  system.  Checks  for  data  stream  integrity  were  utilized 
in  the  form  of  identification  of  a  unique  hexadecimal  identifier  that  indicated  a  new  44  bit 
data  stream.  During  the  research  period  it  was  identified  that  the  data  streams  were 
becoming  corrupted  and  providing  invalid  information  over  some  of  the  read  cycles. 
This  was  contributing  to  errors  propagating  into  the  position  estimates. 
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It  was  determined  that,  due  to  the  errors  developing  in  the  data  streams  over  an  8 
Hz  period,  instead,  a  single  44  bit  data  stream  would  be  used  as  the  average  for  the 
interval.  Additionally,  more  rigorous  data  checks  would  be  placed  on  the  data  to  ensure 
that  it  was  a  complete  and  valid  data  message.  These  checks  included  continuing  to 
identify  a  new  message  by  the  unique  identifier  and  the  use  of  a  checksum  at  the  end  of 
the  data  stream  to  ensure  that  the  preceding  42  bits  of  data  were  correct  and  belonging  to 
the  same  message.  The  new  message  identifier  and  checksum  are  IMU  manufactured 
features  in  the  data  message.  The  use  of  a  single  reading  of  the  100  Hz  data  in  each  8  Hz 
cycle  was  reasoned  to  be  appropriate  because  the  vehicle  dynamics  did  not  change 
rapidly  enough  to  have  significantly  different  readings  from  interval  to  interval  and  the 
sampling  frequency  was  still  greater  than  the  system  dynamics  frequency  as  discussed 
above  to  justify  this  averaging  scheme. 

C.  NAVIGATION  FILTER  CODE 

The  majority  of  the  code  changes  were  made  in  the  navigation  filter  that  contains 
the  EKF  in  the  vehicle.  The  next  few  sections  will  provide  the  methods  by  which  the 
navigation  filter  code  was  changed  with  the  results  shown  in  Chapter  V. 

1.  Gyro  Rate  Bias 

The  EKF  originally  used  was  developed  for  eight  states  as  previously  discussed. 
After  examination  of  the  first  run  utilizing  the  IMU  on  September  7,  it  was  observed  that 
the  filter  was  learning  a  gyro  rate  bias  that  varied  greatly  and  was  significantly  greater 
than  the  manufacturer  specified  maximum  gyro  drift  rate  of  one  degree  per  hour.  Thus,  it 
appeared  as  if  the  filter  was  developing  a  larger  gyro  rate  error  than  the  maximum 
specified.  The  correction  for  this  was  zeroing  out  the  associated  gyro  rate  bias  state  in 
the  filter.  This  was  accomplished  by  setting  the  rows  and  columns  of  the  observation 
matrix  to  zero  and  to  set  the  gyro  rate  bias  state  to  zero  during  the  calculation  cycle. 
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2.  Adaptive  Process  and  Measurement  Noise  Algorithms 

There  has  been  much  work  performed  to  find  optimal  algorithms  that  can 
adaptively  determine  the  values  for  the  process  and  measurement  noise  matrices.  Much 
of  the  available  literature  researched  indicates  that  most  of  the  work  is  based  on  ideas 
proposed  by  Mehra  (1970)  or  work  by  Myers  and  Tapley  (1976).  The  method  originally 
proposed  by  Mehra  (1970)  as  modified  and  presented  by  Busse  et  al.  (2002),  was 
attempted  for  use  in  the  navigation  filter  for  the  ARIES. 

Successful  results  have  not  yet  been  obtained  for  this  application.  The  literature 
suggests  that  the  algorithms  are  highly  dependent  on  tuning  factors  in  the  schemes. 
Therefore,  a  purely  adaptive  routine  has  not  been  found  for  this  particular  application  and 
the  pseudo-adaptive  routine  developed  in  this  work  continues  to  provide  the  best  results 
with  respect  to  navigational  accuracy. 


3.  Pseudo-Adaptive  Measurement  Noise  Matrix 

The  last  major  innovation  to  the  EKF  was  the  tuning  of  the  weights  for 
measurement  noise  values  used  in  the  algorithm.  Prior  to  this  work  the  values  for  both 
process  noise  and  measurement  noise  values  were  assumed  to  be  constant  and  obtained 
experimentally  over  significant  periods  of  operation  with  the  vehicle  and  as  such  were  a 
very  good  first  guess  to  the  optimal  values  to  use  in  the  filter. 


The  measurement  vector  is  a  six  state  vector  in  the  navigation  model  consisting  of 
position,  both  X  and  Y,  forward  and  lateral  speed  over  ground  and  heading  and  gyro  rate. 
It  was  postulated  that  by  selectively  forcing  the  weights  of  specific  states  up  or  down  that 
the  overall  filter  would  place  more  or  less  emphasis  on  the  measurements  for  that  state.  It 
has  been  observed  that  the  Kalman  Gains  are  directly  proportional  to  the  weights  used  for 
the  process  noise  and  inversely  proportional  to  the  weights  for  the  measurement  noise 
values. 


Kalman  Gain  ( L )  oc 


Q 

R 


(22) 
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Equation  (22)  was  used  as  a  guide  to  adjusting  the  weights  from  their  previous 
values  using  the  simulation  EKF  in  MATLAB®  and  the  effect  on  position  estimates 
compared  to  GPS  positions  were  evaluated  for  all  available  data  sets.  Specifically,  the 
pseudo-adaptive  routine  developed  reduced  the  measurement  noise  weighting  values  used 
for  the  position  states,  thereby  forcing  the  filter  to  rely  more  heavily  on  the  information 
from  the  GPS  fixes.  The  measurement  noise  matrix  is  6x6  and  only  the  diagonal  values 
associated  with  the  fifth  and  sixth  state  of  the  measurement  vector  corresponding  to  the 
position  infonnation  is  altered  each  time  step  through  the  filter  based  on  the  number  of 
satellites  seen  by  the  GPS  receiver. 

The  algorithm  developed  for  the  code  altered  the  measurement  noise  values  for 
the  position  states  by  reducing  them  by  an  order  of  magnitude  as  the  number  of  satellites 
visible  from  the  GPS  receiver  increased.  The  original  values  were  used  for  three  or  less 
satellites  visible,  then  reduced  an  order  of  magnitude  for  four  satellites,  the  first  position 
information  in  which  a  GPS  fix  is  declared  and  reduced  another  order  of  magnitude  for 
five  or  more  satellites. 

This  combination  was  run  on  several  of  the  data  sets  from  the  May,  June,  and  July 
timeframe  of  2006  and  the  effects  on  estimated  position  when  compared  with  the  current 
in-vehicle  output  as  well  as  the  GPS  positions.  Figure  9  shows  the  results  of  this 
algorithm  on  the  data  obtained  from  Run  3  on  July  25.  The  improved  filter  perfonnance 
can  clearly  be  seen  when  compared  with  the  GPS  position  after  a  two  kilometer  run. 
After  simulation  runs  on  different  data  sets,  confirming  the  results  observed  in  Figure  9, 
the  algorithm  shown  in  Figure  10  was  implemented  in  the  vehicle  for  field  evaluation. 


28 


POSITION:  GPS,  Filter  Estimated,  In-Vehicle  Estimated 
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Figure  9.  Simulation  with  Pseudo-Adaptive  Algorithm  of  Run  3  -  7/25/06 


//  SPK  081606:  Set  elements  of  R  Matrix  based  on 
NSv  (as  verified  by  SRV  by  08/2006) 
if  (NSv  <=  3) 
nu[5]=1.0; 
nu [6] =1 . 0; 
else  if  (NSv  ==  4) 
nu[5]=0.1; 
nu  [6] =0 . 1; 
else  if  (NSv  >=  5) 
nu [5] =0 . 01; 
nu [ 6] =0 . 01 ; 

/*  Update  Diagonal  R  Matrix  */ 

for ( i=l ; i<=7 ; ++i ) 

R[i] [i]  =  nu [i] ; 


Figure  10.  Pseudo-Adaptive  Algorithm  for  Measurement  Noise 


Originally,  the  filter  and  the  GPS  software  utilized  a  set  point  of  three  GPS 
satellites  as  being  sufficient  fix  quality  to  use  in  the  filter,  however,  it  was  observed  that 
the  use  of  three  satellites  produced  a  significant  error  in  the  solution  as  shown  in  Figure 
1 1 .  Therefore,  the  GPS  code  was  changed  to  declare  GPS  information  available  if  the 
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number  of  visible  satellites  to  the  GPS  receiver  was  greater  than  or  equal  to  four 
satellites.  This  higher  threshold  ensured  the  GPS  receiver  was  obtaining  higher  quality 
fix  information. 
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Figure  1 1 .  Three  vs.  Four  GPS  Satellite  Fix 


In  addition  to  changing  the  measurement  noise  values  for  the  position  states, 
computer  simulations  were  run  altering  only  the  speed  noise  weighting  values  to  observe 
the  effects  on  filter  performance  with  respect  to  position  estimates.  The  weighting  values 
were  changed  individually  for  the  forward  speed  measurement  and  the  lateral  speed 
measurement  by  orders  of  magnitude  until  satisfactory  performance  was  achieved 
qualitatively  in  simulation.  These  values  were  then  checked  against  other  data  sets  to 
ensure  that  acceptable  performance  was  maintained  in  terms  of  accurate  position 
estimation  compared  with  GPS.  The  result  was  that  the  forward  speed  measurement 
noise  weighting  value  was  reduced  by  two  orders  of  magnitude  and  the  lateral  speed 
measurement  noise  weighting  value  was  altered  by  a  single  order  of  magnitude,  placing 

more  reliance  on  the  speed  measurements  overall. 
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Observing  the  resulting  filter  speed  estimates  when  compared  to  actual 
measurements  it  was  seen  that  the  effect  on  the  speed  states  was  reduced  smoothing  of 
the  data.  The  speed  measurements  from  the  RDI  Doppler  and  the  estimations  of  vehicle 
speed  from  the  EKF  in-vehicle  compared  to  the  new  estimates  using  the  modified 
weights  are  shown  in  Figure  12  and  Figure  13.  The  red  cross  marks  are  the  measured 
values  for  speed  from  the  RDI  Doppler  speed  sensor,  the  blue  dots  are  the  in-vehicle  EKF 
estimates  from  that  run  and  the  green  dots  are  the  new  estimates  from  the  simulation  EKF 
after  modifying  the  noise  values. 
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Figure  13.  Lateral  Speed  Filter  Results  of  Run  3  -7/25/06 

The  observation  drawn  here  is  that  what  appears  to  be  noise  in  the  measurements 
and  captured  as  such  in  the  system  model  could  be  actual  slight  changes  in  vehicle  speed 
due  to  external  forces,  possibly  from  wave  or  current  action  or  alterations  in  which  the 
thrusters  propel  the  vehicle  through  the  water.  It  would  thus  introduce  slight  errors  to 
model  this  information  as  noise  vice  actual  variations  in  the  speed  measurement. 

An  additional  feature  of  the  fusion  filter  with  the  pseudo-adaptive  algorithm  is 
that  it  is  more  responsive  to  the  heading  bias  learning  process.  This  phenomenon  can  be 
observed  in  Figure  14;  while  not  as  smooth,  it  converges  to  the  actual  necessary 
initialization  error  quicker  than  previous  runs  due  to  the  increased  weighting  on  the 
position  measurements.  The  oscillatory  nature  of  the  heading  bias  state  early  in  the  run  is 
from  speed  information  that  is  approximately  zero  when  the  vehicle  is  on  the  surface  at 
the  beginning  of  a  run  awaiting  mission  execution  commands  but  with  the  navigation 
process  running. 
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Bias^:  sim  estimated  [green],  exp  estimated  [blue] 


Figure  14.  EKF  Bias  Learning  with  Pseudo-adaptive  R  Matrix  -  7/25/06 

Lastly,  it  was  observed  that  once  the  vehicle  started  its  thrusters  and  began  a 
mission  with  an  increase  in  speed  the  heading  bias  converges  quickly  to  a  steady  state 
value  that  is  very  close  to  the  final  learned  heading  bias.  This  is  due  to  the  fact  that 
accurate  position  information  is  still  being  obtained  via  GPS  fixes  prior  to  submerging 
and  there  is  a  non-zero  speed  component  with  a  relatively  accurate  heading  input.  The 
heading  bias  as  the  only  unobserved  state  during  this  time  frame  which  converges  rapidly 
within  this  estimator  with  all  other  states  observed. 

One  last  navigational  modification  was  made  based  on  this  last  observation  after 
the  August  24  run.  A  time  delay  of  10  seconds  was  inserted  into  the  executive  process  of 
ARIES  to  delay  submerging  after  thrusters  were  initiated.  With  this  delay,  the  filter 
would  have  all  the  state  information  necessary  for  accurate  heading  bias  estimation 
thereby  eliminating  needs  for  subsequent  pop-ups.  The  value  of  this  would  be  enhanced 
mission  effectiveness  by  eliminating  the  need  for  a  GPS  fix  to  correct  the  heading  bias 
prior  to  entering  an  operating  area  to  conduct  its  mission. 
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V.  EXPERIMENTS  AND  RESULTS 


A.  INTRODUCTION 

In  order  to  understand  the  effects  of  the  implementation  of  the  IMU  and 
modifications  to  the  vehicles  software  architecture  presented  in  Chapters  III  and  IV, 
experiments  with  the  vehicle  were  conducted  in  Monterey  Bay  with  the  ARIES  vehicle 
from  September  2005  through  September  2006.  These  tests  were  established  to  run  the 
vehicle  in  a  controlled  geometry  with  varying  GPS  pop-ups  in  order  to  quantify  the 
navigational  position  error  between  the  filter  estimate  and  the  vehicles  position  as 
determined  by  the  GPS  receiver. 

B.  EXPERIMENT  GEOMETRIES 

Experiment  geometries  were  maintained  rather  simple  and  consistent  to  provide  a 
basis  for  rapid  qualitative  comparison  in-situ  as  well  as  post  analysis  quantitative 
comparison.  This  was  done  by  utilizing  a  straight  run  of  lengths  of  one  kilometer  initially 
and  then  eventually  expanding  to  two  kilometer  runs  and  finally  a  four  kilometer  run  to 
validate  the  findings.  The  runs  were  oriented  on  a  North-South  axis  for  simplicity  of 
track  planning  and  safety  of  vehicle  in  order  to  run  parallel  to  the  shoreline  in  Monterey 
Bay.  The  majority  of  runs  were  set  to  run  at  a  depth  of  three  to  four  meters  unless  other 
testing  requirements  dictated  that  the  vehicle  run  on  altitude  control  utilizing  the  bottom 
topography  vice  a  commanded  depth  input. 

1.  GPS  Pop-up  Maneuvers 

The  number  of  GPS  pop-ups  varied  from  as  many  as  approximately  eight  on  the 
early  runs  to  two  on  the  final  runs  towards  the  end  of  the  test  period.  The  use  of  many 
GPS  pop-ups  allowed  for  statistical  quantification  of  the  mean  error  between  filter 
estimate  of  position  and  actual  position.  Additionally,  the  use  of  many  pop-ups  allowed 
for  analysis  of  filter  performance  with  respect  to  estimating  the  gyro  rate  bias  and  the 
heading  bias  throughout  the  run. 
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2.  Geometry  Modifications 

There  were  two  modifications  to  the  geometry  described  above  during  the 
experimental  phase.  First,  a  multi-heading,  multi-turn  geometry  was  utilized  in  order  to 
qualitatively  observe  performance  of  the  vehicle  IMU  and  filter  under  a  rather  intense 
maneuvering  scheme.  This  run  can  be  seen  below  in  Figure  15.  The  IMU  performed 
excellently  as  a  heading  reference  on  multiple  courses  over  this  run  without  incurring 
additional  errors  that  otherwise  may  have  been  incurred  with  a  compass.  Secondly,  the 
final  four  kilometer  validation  run  was  performed  on  an  East-West  cardinal  heading 
following  a  two  kilometer  North-South  run  to  demonstrate  that  the  results  were 
independent  of  vehicle  heading.  Additionally,  this  ensured  no  artificial  bias  may  have 
existed  in  the  results  obtained  by  the  selection  of  repeated  North-South  runs. 


d051806_04.d  Path  Plot 


E 

X 


I 

I 

1 

I 

1 

“ 

i 

12650 

i 

i 

i 

i 

1 

1 

1 

1 

( 1 
.1 

1 

-T- 

-7 " 

U] 

1 

i 

i 

i 

i 

12600 

i 

T 

1 

1 

r 

i 

1 

| 

\t> 

1 

•V - T - 

1 

i 

r 

i 

12550 

1 

_  _L 

1 

i 

L 

1 

j 

1 

| 

1 

V - !  — 

i 

L 

1 

12500 

1 

"  T  - 

i 

1 

j 

1 

V!> - 1--- 

1 

1 

i 

1 

— 

12450 

i 

-  +  - 

l 

1 

- 1 - 

1 

\l 

- A| - 

j 

1 

- + - 

1 

1 

- 1 - 

1 

12400 

l 

i 

i 

1 

1 

1 

— 

1 

1 

1 

1 

1 

1 

12350 

i 

'  T 

1 

r 

1 

1 

T 

1 

r 

1 

i 

1 

*  1 

i 

12300 

-l 

1 

L 

1 

1 

— 

X 

l_ 

1 

1 

1 

1 

Waypoints 

12250 

1 

1 

1 

1 

—  ARIES  Track 

— 

1 

1 

1 

Nav  Filt  Est  w/GPS  NsV  >4 

12200 

-  +  - 

- 1 - 

18 

GPS  Fix 

— 

6200  6300  6400  6500  6600  6700  6800 


Y[m] 


Figure  1 5 .  Multi-Heading  Run  Geometry  -  5/1 8/06 
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C.  EXPERIMENT  RESULTS 

The  following  sections  will  present  the  results  of  the  modifications  discussed  in 
Chapters  III  and  IV  following  the  same  chronology. 

1.  Original  Navigation  Filter 

The  original  navigation  filter  utilized  a  compass  as  the  heading  reference  input 
and  the  navigational  errors  incurred  were  relatively  large.  Figure  16.  and  Figure  17. 
show  the  evolution  of  the  mean  errors  with  each  major  change  to  the  navigation  filter  as 
well  as  the  mean  error  per  unit  distance  traveled. 


Navigation  Accuracy  Trend 


Figure  16.  Evolution  of  Navigation  Filter  Errors 
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Figure  17.  Expanded  Evolution  of  Navigation 


Figure  18  and  Figure  19  illustrate  typical  tracks  from  the  use  of  the  compass  as  a 
heading  input  to  the  navigation  filter. 


Figure  18.  Compass  Based  Track- 6/10/05 
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Figure  19.  Compass  Based  Track  -  8/9/05 


The  navigational  errors  were  large  and  highly  dependent  on  the  quality  of  the 
compass  calibration  and  the  ability  of  the  heading  bias  learning  process  to  correctly 
obtain  the  initialization  error.  With  the  compass  as  the  heading  reference,  the  baseline 
mean  error  was  70.59  meters  and  the  error  per  unit  distance  traveled  was  66.67%. 


2.  Initial  Hardware  Modification 

The  installation  of  the  Honeywell  IMU  replaced  the  Systran  Donner  Motion  Pak 
IMU  as  the  source  of  the  gyro  rate  which  when  integrated  would  provide  the  heading 
input  for  the  filter  to  produce  better  estimates  of  position  based  on  the  equations  of 
motion.  The  first  run  with  the  IMU  saw  significant  improvements  in  the  quality  of  the 
position  estimates.  Figure  20  shows  the  results  of  this  run. 
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Figure  20.  Initial  Post-IMU  Install  Run  -  9/7/05 

There  was  a  significant  difference  in  navigation  performance  with  the 
implementation  of  the  IMU.  Overall  mean  navigational  errors  were  reduced  to  8.04 
meters  and  the  error  per  unit  distance  traveled  had  been  reduced  to  3.04  %  for  this  initial 
run  with  installation  of  the  IMU. 

3.  Gyro  Rate  Bias  Modification 

The  corresponding  reduction  in  navigational  error  was  an  order  of  magnitude. 
Analysis  of  the  September  7th  run  revealed  that  the  rate  bias  state  was  producing  rather 
large  values,  which  when  compared  to  the  manufacturers  published  gyro  drift  rate  for  the 
IMU  of  one  degree  per  hour  indicated  that  there  were  additional  errors  being  induced  or 
captured  by  this  state  through  the  vehicle  dynamics.  The  gyro  rate  biases  obtained  from 
the  initial  runs  with  the  IMU  are  shown  in  Figure  21  and  Figure  22. 
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Nav  Filter  Bf 


Figure  2 1 .  Navigation  Filter  Rate  Bias  -  9/7/05 


Figure  22.  Navigation  Filter  Rate  Bias  -  5/12/06 
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Based  on  the  observations  of  Figure  2 1 .  and  Figure  22.  above  it  was  determined  to 
force  the  rate  bias  state  to  zero  within  the  filter  to  eliminate  erroneous  learning  and  thus 
introducing  errors  into  the  filter.  The  mean  positional  errors  during  the  time  period  that 
the  rate  bias  learning  was  activated  and  with  the  IMU  installed  was  19.23  m  with  an 
associated  8.69  %  error  per  unit  distance  traveled.  There  was  significant  variability  in  the 
results  as  seen  from  the  raw  data  in  Table  2  of  Appendix  A. 

With  the  rate  bias  state  set  to  zero  in  the  filter  as  discussed  in  Chapter  IV,  the 
ARIES  was  run  again  on  May  25,  June  14  and  June  15  without  any  other  changes  to  the 
navigation  filter.  Figure  23  is  a  typical  result  of  the  runs  during  this  time  period. 
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Figure  23.  Typical  Run  with  Rate  Bias  set  to  Zero  -  6/14/06 

The  mean  navigational  errors  during  this  period  were  16.09  meters,  which  is  not  a 
significant  change  but  the  mean  error  as  a  percentage  of  the  distance  traveled  decreased 
to  5.63  %.  Again,  there  was  much  variability  observed  in  the  results  but  the  trend  is 
increasing  downward. 
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4.  IMU  Code  Averaging  Scheme 

During  the  June  runs  it  was  discovered  that  there  were  errors  being  introduced 
into  the  44  bit  data  streams  containing  the  measurements  from  the  IMU.  These  errors, 
under  the  original  architecture  of  the  code,  were  being  averaged  over  the  8  Hz  period. 
The  erroneous  heading  rate  was  then  being  integrated  for  heading  for  use  by  the 
navigation  filter.  The  source  of  the  errors  was  not  discovered  and  the  IMU  code  was 
changed  as  discussed  Chapter  IV.  The  ARIES  was  run  on  July  19  and  July  25  with  the 
results  shown  as  follows  in  Figure  24  and  Figure  25. 
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Figure  24.  ARIES  Run  with  IMU  change  -  7/19/06 
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Figure  25.  ARIES  Run  with  IMU  change  -  7/25/06 


A  noticeable  improvement  was  realized  with  this  methodology  which  utilized  a 
single  good  sample  from  the  8  Hz  calculation  interval.  The  underlying  assumption  made 
was  that  the  true  vehicle  dynamics  did  not  change  quickly  enough  and  that  a  single 
measurement  made  over  the  interval  would  be  representative  of  the  interval.  During  this 
period  of  runs,  the  mean  navigational  error  improved  to  7.92  meters  and  error  per  unit 
distance  traveled  was  0.81  %.  The  overall  goal  of  one  percent  of  distance  traveled  had 
been  achieved  but  a  review  of  Table  4  shows  that  there  were  some  values  that  were  right 
above  the  desired  level. 


5.  Pseudo-Adaptive  Measurement  Noise  Modification 

The  result  of  implementing  the  pseudo-adaptive  noise  algorithm  were  errors 
consistently  less  than  one  percent  error  over  the  distance  traveled.  An  initial  two 
kilometer  run  was  conducted  on  August  24  to  verify  the  results  compared  to  previous 
runs  of  the  same  geometry  and  to  validate  what  had  been  observed  in  simulation  shown 
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in  Figure  9.  The  total  error  after  the  two  kilometers  was  5.36  meters  for  a  resulting 
percentage  error  of  0.23%  of  the  distance  traveled.  This  run  is  shown  below  in  Figure  26. 
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Figure  26.  2  km  Run  with  Pseudo-Adaptive  Algorithm  -  8/24/06 


A  second  run  was  then  conducted  to  confirm  the  results  obtained.  For  this  run  the 
geometry  would  be  rotated  so  that  the  runs  would  occur  on  East-West  cardinal  headings. 
Additionally,  the  run  would  be  doubled  to  four  kilometers  to  observe  how  the  fdter 
handled  the  accumulated  error  due  to  the  drift  of  the  IMU  gyros  during  this  extended  run. 
The  four  kilometer  run  is  shown  in  Figure  27. 
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Figure  27.  4  km  Run  with  Pseudo-Adaptive  Algorithm  -  8/24/06 


The  result  for  this  run  was  an  accumulated  position  error  of  14.16  meters  over  the 
four  kilometer  run,  resulting  in  a  percentage  error  of  0.33%.  This  geometry  demonstrated 
that  the  accuracy  of  the  navigation  was  independent  of  any  particular  track  heading. 


6.  Surface  Time  Delay  Implementation 

An  initial  ten  second  surface  delay  was  inserted  into  the  operating  profile  for  the 
vehicle  and  tested  on  October  17.  This  based  on  observations  made  during  simulation  on 
the  July  25  run  as  discussed  in  Chapter  IV.  The  results  of  the  surface  delay  are  shown 
below  in  Figure  28.  An  enhanced  view  of  the  initial  surface  run  is  illustrated  in  Figure 
29.  The  heading  bias  with  accurate  position  information  and  non-zero  speed  information 
approaches  a  final  steady  state  value,  thus  validating  the  hypothesis  discussed  in  Chapter 
IV.  The  result  of  learning  the  bias  quickly  before  submerging  is  that,  if  given  the  proper 
time,  the  filter  estimates  will  converge  with  actual  position  by  GPS  which  can  be 
observed  in  Figure  30. 
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Bias  Learning  Rate 


Figure  28.  Bias  Learning  Rate  after  10  second  surface  delay  -  10/17/06 


Figure  29.  Enhanced  view  of  Bias  Learning  -  10/17/06 
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Figure  30.  EKF  Estimate  and  GPS  Convergence  -  10/17/06 

The  result  of  implementing  the  surface  time  delay  and  thus  learning  the  heading 
bias  while  still  surfaced  is  observed  with  an  increase  in  accuracy  of  the  initial  GPS  pop¬ 
up  maneuver.  The  filter  estimate  when  compared  to  GPS  position  for  the  first  GPS  pop¬ 
up  can  be  seen  in  Figure  3 1 . 


Figure  3 1 .  Initial  GPS  Pop-Up  Maneuver  with  Surface  Time  Delay  -  10/17/06 
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The  work  performed  in  this  research  has  resulted  in  excellent  navigational 
accuracy  for  the  ARIES  AUV.  It  will  allow  the  ARIES  to  be  used  for  work  in  which 
precise  position  infonnation  must  be  made  available  to  the  vehicle  and  will  allow 
research  that  had  been  unable  to  be  accomplished. 
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VI.  CONCLUSIONS  AND  RECOMMENDATIONS 


A.  CONCLUSIONS 

This  research  demonstrated  that  good  navigational  accuracy  could  be  achieved 
through  the  use  of  a  low  cost  IMU  coupled  with  a  properly  tuned  EKF.  A  one  percent 
error  as  a  function  of  distance  traveled  was  achievable  in  this  research.  This  was  shown 
to  be  possible  through  the  use  of  a  higher  quality  IMU  that  had  a  lower  gyro  drift  rate 
thus  inducing  smaller  errors  over  time  in  the  heading  input  for  the  equations  of  motion 
for  the  vehicle. 

A  pseudo-adaptive  routine  was  developed  that  demonstrated  how  infonnation 
obtained  from  measurements  could  be  maximized  based  on  the  quality  of  the  infonnation 
by  properly  weighting  the  measurement  noise  values  for  certain  states.  In  this  manner 
better  estimates  of  position  were  obtained  as  compared  to  the  GPS  positions  with  overall 
smaller  enors.  The  better  estimates  of  position  were  the  result  of  a  faster  convergence  to 
a  particular  heading  bias  for  a  given  run.  The  pseudo-adaptive  routine  forced  the  heading 
bias  state  in  the  system  to  be  more  reactive  and  dynamic  due  to  changes  in  the  position, 
heading  and  speed  states. 

It  was  also  shown  how  a  navigation  filter  could  be  enhanced  by  changes  in 
operating  procedures.  This  was  demonstrated  through  the  surface  time  delay  in  which  a 
highly  reactive  heading  bias  state  could  converge  quickly  to  the  necessary  value  by 
increasing  speed  while  still  on  the  surface  receiving  GPS  information  during  the  initial 
period  of  a  run.  The  result  is  that  with  the  heading  bias  learned  prior  to  initial 
submergence,  subsequent  vehicle  pop-up  maneuvers  to  leam  the  heading  bias  could  be 
eliminated. 

B.  RECOMMENDATIONS 

This  research  stimulated  many  questions  regarding  the  accuracy  of  underwater 
vehicle  navigation.  Some  of  these  questions  were  answered  by  this  thesis,  more  remain. 
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This  work  was  based  on  establishing  a  GPS  position  as  ground  truth;  however,  there  are 
inaccuracies  within  this  system  that  would  leave  an  absolute  position  error  that  should  be 
dealt  with. 

The  navigation  filter  during  the  course  of  the  research  was  modified  from  an  eight 
state  filter  to  a  seven  state  filter  through  zeroing  the  rate  bias  state.  The  code  and  filter 
should  be  redesigned  to  remove  the  zeroed  state  for  the  rate  bias  into  a  better  defined 
seven  state  model. 

This  work  focused  on  utilizing  a  single  specific  channel  from  the  IMU,  i.e.  the 
yaw  rate  that  was  then  integrated  to  provide  a  good  heading  reference.  The  IMU  outputs 
three  dimensional  body  linear  accelerations  and  angular  rates  that  could  be  used  to  further 
enhance  the  accuracy  of  the  navigational  estimates  in  both  the  horizontal  and  vertical 
planes.  The  method  proposed  based  on  current  system  architecture  is  to  integrate  the 
accelerations  in  the  IMU  code  at  100  Hz  and  output  the  velocity  components  at  8  Hz 
intervals.  These  velocities  could  then  be  used  in  place  of  the  velocities  provided  by  the 
RDI  Doppler  in  the  EKF. 

Adopting  a  two  IMU  system  and  removing  the  RDI  Doppler  as  the  primary  speed 
sensor.  Eliminating  the  RDI  Doppler  as  an  onboard  sensor  has  the  following  benefits: 

■  Hotel  load  reduced,  increasing  mission  endurance. 

■  Reduced  electrical  noise  impacts  on  Forward  Look  Sonar. 

■  Reduced  capital  cost. 

o  $35k  (RDI  Doppler) +  $  15k  (IMU)  vs. 
o  $  1 5k  (Primary  IMU)  +  $  1 5k  (Backup  IMU) 

■  Eliminates  water  column  constraints  by  eliminating  need  to  see  ocean  bottom. 

■  Increases  payload  space  for  other  components  or  reduces  overall  size  of  AUV. 

■  Enhanced  mission  flexibility  due  to  above. 

This  work  discovered  that  a  pseudo-adaptive  measurement  noise  scheme  worked 
rather  well  in  reducing  the  overall  navigational  error.  While  some  work  was  performed 


52 


to  adapt  an  algorithm  by  Busse  et  al.,  (2002)  into  the  EKF,  little  success  was  made  and 
the  filter  solutions  diverged.  Additional  work  ought  to  be  perfonned  to  determine  an 
adaptive  process  and  measurement  noise  algorithm  that  will  perfonn  within  the  current 
architecture  of  this  EKF  application. 

There  is  potential  work  that  warrants  attention  in  post-process  smoothing  of  the 
vehicle  track.  In  this  manner  by  using  more  precise  information  from  position 
measurements  made  through  GPS  the  track  that  the  vehicle  actually  ran  between  GPS 
pop-up  maneuvers  may  be  estimated.  It  can  be  seen  from  previous  figures  that  the 
vehicle  bases  its  control  action  from  position  estimates  made  through  the  navigation  filter 
and  that  there  are  discontinuities  in  the  solution  from  the  (/-/)  time  step  to  the  (/)  time 
step  at  which  a  GPS  measurement  is  made.  By  using  a  fixed-interval  smoothing  routine 
as  proposed  by  Bar-Shalom  (2001)  the  vehicle  track  may  be  post-processed  for  a  more 
accurate  representation  of  true  vehicle  position  between  GPS  fixes. 
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APPENDIX  A:  EXPERIMENT  DATA 


The  mean  errors  and  associated  errors  per  unit  distance  traveled  for  runs  analyzed 
during  this  research  are  summarized  in  tables  1  through  5.  The  tables  are  divided  into 
each  specific  evolution  of  the  vehicle  for  navigation  improvements. 


Date 

NavD  File 

d  File 

Delta  X 

Delta  Y 

Error  (m) 

Error  % 
Dist 

Traveled 

6/10/2005 

NavD  061005  01 

d061005  01 

-45.49 

-65.53 

79.77 

116.29 

50.04 

-60.47 

78.49 

18.33 

Totals 

4.55 

-126.00 

158.26 

134.62 

Means 

2.27 

-63.00 

79.13 

67.31 

StDev 

67.55 

3.57 

0.90 

69.27 

8/9/2005 

NavD  080905  01 

d080905  01 

-1.15 

28.35 

28.37 

54.94 

-2.63 

49.49 

49.56 

81.63 

-12.39 

116.11 

116.77 

62.15 

Totals 

-16.17 

193.96 

194.71 

198.73 

Means 

-5.39 

64.65 

64.90 

66.24 

StDev 

6.11 

45.80 

46.15 

13.80 

Table  1 .  Pre-IMU  ARIES  Data 
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Date 

NavD  File 

d  File 

Delta  X 

Delta  Y 

Error  (m) 

Error  % 
Dist 

Traveled 

9/7/2005 

NavD  090705  01 

0090705  05 

4.27 

9.19 

10.14 

4.84 

-0.14 

5.95 

5.95 

1.24 

Totals 

4.14 

15.14 

16.09 

6.08 

Means 

2.07 

7.57 

8.04 

3.04 

StDev 

3.12 

2.29 

2.96 

2.54 

5/12/2006 

NavD  051206  02 

d051206  02 

3.85 

-0.07 

3.85 

1.88 

-0.08 

0.87 

0.87 

0.48 

-0.32 

-1.44 

1.47 

0.67 

0.58 

-4.73 

4.77 

2.34 

0.81 

-6.24 

6.30 

3.17 

-1.85 

-12.34 

12.48 

6.25 

NavD  051206  03 

d051206  03 

14.22 

-55.17 

56.97 

29.13 

-25.92 

49.32 

55.71 

27.52 

7.39 

53.30 

53.81 

23.99 

7.89 

-1.62 

8.06 

3.48 

1.94 

-32.71 

32.76 

17.09 

-10.45 

-25.79 

27.82 

14.33 

8.32 

-31.07 

32.16 

17.26 

NavD  051206  03 

d051206  04 

0.95 

48.04 

48.05 

8.32 

Totals 

7.33 

-19.65 

345.08 

155.92 

Means 

0.52 

-1.40 

24.65 

11.14 

StDev 

9.56 

32.24 

21.89 

10.31 

5/18/2006 

NavD  051806  03 

d051806  04 

3.37 

-1.32 

3.62 

0.71 

-6.00 

-2.70 

6.58 

3.60 

-0.93 

-0.47 

1.04 

0.42 

-6.59 

10.30 

12.22 

7.15 

Totals 

-10.15 

5.81 

23.46 

11.88 

Means 

-2.54 

1.45 

5.87 

2.97 

StDev 

4.68 

5.97 

4.80 

3.13 

Table  2.  Post-IMU  Installation  ARIES  Data 
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Date 

NavD  File 

d  File 

Delta  X 

Delta  Y 

Error  (m) 

Error  % 
Dist 

Traveled 

5/25/2006 

NavD  052506  03 

d052506  03 

1.85 

0.32 

1.88 

0.26 

NavD  052506  04 

d052506  04 

4.12 

-2.47 

4.80 

0.61 

Totals 

5.97 

-2.15 

6.68 

0.87 

Means 

2.98 

-1.08 

3.34 

0.43 

StDev 

1.61 

1.97 

2.07 

0.25 

6/14/2006 

NavD  061406  02 

d061406  02 

2.33 

-8.76 

9.07 

1.74 

5.10 

-3.18 

6.01 

1.36 

NavD  061406  04 

d061406  04 

10.30 

25.36 

27.37 

7.88 

1.86 

16.00 

16.10 

6.43 

2.00 

17.89 

18.01 

8.54 

-0.11 

12.32 

12.32 

5.95 

15.45 

14.11 

20.93 

8.19 

-1.29 

30.81 

30.84 

15.27 

-5.84 

19.96 

20.80 

11.12 

-2.02 

11.10 

11.28 

5.87 

NavD  061406  06 

d061406  06 

-1.05 

45.52 

45.53 

6.82 

-0.02 

4.17 

4.17 

1.92 

5.36 

12.83 

13.91 

7.65 

1.85 

8.62 

8.81 

4.81 

0.73 

0.58 

0.93 

0.42 

-5.19 

-10.23 

11.47 

5.98 

0.76 

-17.55 

17.57 

8.24 

Totals 

30.22 

179.54 

275.10 

108.19 

Means 

1.78 

10.56 

16.18 

6.36 

StDev 

5.19 

15.76 

10.90 

3.73 

6/15/2006 

NavD  061506  03 

d061506  04 

3.88 

18.08 

18.49 

2.92 

NavD  061506  04 

d061506  05 

1.67 

-37.58 

37.62 

6.21 

Totals 

5.55 

-19.50 

56.11 

9.13 

Means 

2.78 

-9.75 

28.05 

4.57 

StDev 

1.56 

39.36 

13.53 

2.33 

Table  3.  ARIES  Data  with  Gyro  Rate  Bias  Set  to  Zero 
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Date 

NavD  File 

d  File 

Delta  X 

Delta  Y 

Error  (m) 

Error  %  Dist 
Traveled 

7/19/2006 

NavD  071906  03 

d071906  03 

5.07 

2.47 

5.63 

0.70 

-6.10 

-5.58 

8.27 

1.24 

NavD  071906  06 

d071906  07 

3.81 

1.29 

4.02 

0.70 

1.52 

-3.56 

3.87 

0.74 

Totals 

4.29 

-5.39 

21.79 

3.39 

Means 

1.07 

-1.35 

5.45 

0.85 

StDev 

5.01 

3.84 

2.04 

0.26 

7/25/2006 

NavD  072506  01 

d072506  01 

2.56 

-3.36 

4.22 

0.42 

NavD  072506  02 

d072506  02 

5.16 

-0.57 

5.19 

0.76 

NavD  072506  03 

d072506  03 

23.04 

-7.62 

24.27 

1.13 

Totals 

30.75 

-11.54 

33.67 

2.30 

Means 

10.25 

-3.85 

11.22 

0.77 

StDev 

11.15 

3.55 

11.30 

0.36 

Table  4. 


ARIES  Data  with  IMU  Data  Averaging  Change 


Date 

NavD  File 

d  File 

Delta  X 

Delta  Y 

Error  (m) 

Error  %  Dist 
Traveled 

8/24/2006 

NavD  082406  05 

d082406  05 

0.71 

-5.32 

5.36 

0.23 

NavD  082406  06a 

d082406  06a 

13.96 

2.35 

14.16 

0.33 

Totals 

14.68 

-2.96 

19.52 

0.56 

Means 

7.34 

-1.48 

9.76 

0.28 

StDev 

9.37 

5.42 

6.22 

0.07 

9/12/2006 

N  avDO 9 1 20 6  0 1 

d091206  02 

3.18 

9.61 

10.13 

0.45 

Table  5.  ARIES  Data  with  Pseudo-Adaptive  Algorithm 
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APPENDIX  B:  SIMULATION  EKF  CODE 


The  following  MATLAB®  code  was  used  for  running  simulations  on  the  data 
sets.  The  original  code  was  developed  by  A.  Healey  with  modifications  made  by  H.S. 
Kim  and  S.  Vonheeder  during  the  course  of  this  work.  The  codes  contained  in 
Appendices  B  and  C  are  intended  to  run  in  MATLAB®  with  a  specific  data  set  structure 
from  the  ARIES  vehicle.  These  data  sets  may  be  obtained  from  the  NPS  Center  for  AUV 
Research  or  the  codes  can  be  tailored  to  run  with  specific  data  sets  to  provide  the  correct 
state  information. 

S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S- 

ooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo 

%  AUV  Research  Center  % 

%  Extended  Kalman  Filter  For  ARIES  AUV  % 

%  Edited:  11/9/06  % 

ooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo 

close  all;  clear  all;  clc 

%%  Load  data  from  Desired  Run  with  Aries  using  d  and  NavD  Files 
%  generate  default  file  name  for  dialog  box 
def DateFields  =  datevec (date) ; 

defYear  =  sprintf (' %02d def DateFields ( 1 )  -  2000); 
defMon  =  sprintf (' %02d def DateFields (2 )) ; 
defDay  =  sprintf (' %02d def DateFields ( 3 )) ; 
defDateStamp  =  [defMon  defDay  defYear  '  0 1 . d ' ]  ; 

defNameD  =  [ ' d '  defDateStamp]; 

%  get  d-file  name  from  dialog  box 
fields  =  { ' D  File:'}; 
boxTitle  =  'Get  Data  File'; 
lineNo  =  1; 

defaultlnput  =  {defNameD}; 

input  =  inputdlg ( fields ,  boxTitle,  lineNo,  defaultlnput,  'on'); 
if  (  isempty (input)  ) 

disp('No  data  file  selected.  Exit  program.'); 
return; 

else 

dFileName  =  input{l}; 

end; 

%  get  NavD-file  name  from  dialog  box 
fields  =  {'NavD  File:'}; 
boxTitle  =  'Get  Nav  Data'; 
lineNo  =  1; 

defNameNavD  =  ['NavD  '  dFileName (2 : 10)  ' . d ' ] ; 

defaultlnput  =  {defNameNavD}; 

input  =  inputdlg ( fields ,  boxTitle,  lineNo,  defaultlnput,  'on'); 
if  (  isempty (input)  ) 
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disp('No  data  file  selected.  Exit  program.'); 
return ; 

else 

dFileNameNav  =  input{l}; 

end; 

d  =load (dFileName) ; 

Nav  =load (dFileNameNav) ; 

%%  Data  Assignment  to  variables 
sss  =Nav (1,2) ; 

eee  =length (Nav ( : , 2 ) ) +Nav (1,2) -1 ; 

Iran  =max (size (Nav) ) ; 

startSample=l ; 
endSample=mmm; 


d  khs  ( : 

,1)  = 

Nav ( : , 2 1 )  ; 

o, 

o 

gpsStatus  check  ! ->  ok) 

d  khs  ( : 

,2)  = 

Nav ( : , 32 ) 

o, 

o 

IMU  theta  [rad] 

d  khs  ( : 

,3)  = 

Nav ( ; , 31 )  ; 

o, 

o 

IMU  phi  [rad] 

d  khs  ( : 

,4)  = 

Nav ( : , 54 )  ; 

o, 

o 

IMU  psi  [rad] 

d  khs ( : 

,5)  = 

Nav ( : , 8 )  ; 

o, 

o 

yaw  rate  [rad/sec] 

d  khs  ( : 

,6)  = 

Nav ( : , 10)  ; 

g, 

o 

u  [m/sec] 

d  khs  ( : 

,7)  = 

Nav ( : , 1 1 )  ; 

g, 

o 

v  [m/sec] 

d  khs  ( : 

,8)  = 

Nav ( ; , 12 )  ; 

g, 

o 

w  [m/sec] 

d  khs  ( : 

,9)  = 

Nav ( : , 47 )  ; 

g, 

o 

GPS  longitude 

d  khs ( : 

,  10) 

=Nav ( : , 46) ; 

g, 

o 

GPS  latitude 

d  khs  ( : 

,11) 

=Nav ( : , 35 )  ; 

g, 

o 

Nav  lat  (X  [m]  ) 

d  khs  ( : 

,  12) 

=Nav ( : , 36) ; 

g, 

o 

Nav  long  (Y  [m] ) 

d  khs  ( : 

,  13) 

=Nav ( : , 37 )  ; 

g, 

o 

Nav  heading  [rad] 

d  khs  ( : 

,14) 

=Nav ( : , 38 )  ; 

g, 

o 

Nav  yaw  rate  [rad/sec] 

d  khs  ( : 

,  15) 

=Nav ( : , 39) ; 

g, 

o 

Nav  u  [m/sec] 

d  khs  ( : 

,16) 

=Nav ( : , 4  0 )  ; 

g, 

o 

Nav  v  [m/sec] 

d  khs  ( : 

,17) 

=Nav ( : , 42 ) ; 

g, 

o 

Nav  Bias  r  [rad/sec]  (experimentally 

%  estimated  Bias  psi) 

d  khs  ( : 

,  18) 

=Nav ( : , 4 1 )  ; 

g, 

o 

Nav  Bias  psi  [rad]  (experimentally 

%  estimated  Bias  r) 

Bias  psi 

=Nav ( : , 4 1 )  ; 

g, 

o 

[rad]  (experimentally  estimated 

Bias  psi) 

Bias  r 

=Nav ( : , 42 )  ; 

g, 

o 

[rad/sec]  (experimentally  estimated 

Bias  r) 

gpsStatus  = 

d  khs  {:,!); 

pitch 

= 

d  khs ( : , 2 )  ; 

roll 

= 

d  khs ( : , 3 ) ; 

heading 

= 

d  khs  (  : , 4 )  ; 

yaw  rate 

d  khs ( : , 5 )  ; 

ug 

= 

d  khs ( : , 6) ; 

vg  RDI 

= 

d  khs  {:,!); 

vg 

= 

d_khs ( : , 7) -1 

0*d  khs 

: ,  5 )  ; 
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wg 

=d  khs  (  :  , 8 ) 

long 

=d  khs ( : , 9) 

lat 

=d_khs ( : , 10 

NsV 

=Nav ( : , 23) ; 

ll=long (startSample)  ; 

12=lat (startSample) ; 
long=long-ll*ones (length (lat)  ,  1)  ; 
lat  =lat-12*ones (length (lat)  ,  1) ; 

for  i=l : mmm, 

if  (gpsStatus ( i , 1 ) ==0 ) 

long ( i , 1 ) =0 . 0 ;  lat ( i , 1 ) =0 . 0 ; 

end 

end 

NsV_3=f ind (NsV==3  &  Nav ( : , 46) ~=0) ; 

GPS_3 ( : , 1) =Nav (NsV_3, 4  6)  ; 

GPS_3 ( : , 2) =Nav (NsV_3, 47) ; 

NsV  4=f ind (NsV==4  &  Nav ( : , 46) ~=0) ; 

GPS_4 ( : , 1) =Nav (NsV_4, 46) ; 

GPS_4 ( : , 2) =Nav (NsV_4, 47)  ; 

NsV  5=f ind (NsV>=5  &  Nav ( : , 46) ~=0) ; 

_ GPS_5 ( : , 1) =Nav (NsV_5, 46) ; 

GPS_5 ( : , 2) =Nav (NsV_5, 47) ; 

dt=l / 8 ; 

t=0 : dt : ( length (ug) -1 ) *dt ; 

nheading  =  zeros ( 1 , length (heading) ) ; 
nheading(l)  =  heading (1); 

for  i=2 : length (heading) 

if  abs (heading ( i )  -  heading ( i — 1 ) )  <=  pi 

nheading (i)  =  nheading ( i — 1 )  +  heading (i)  -  heading ( i — 1 ) ; 

end 

if  heading (i)  -  heading (i-1)  >  pi 

nheading (i)  =  nheading (i-1)  +  heading (i)  -  heading (i-1)  -  2*p 

end 

if  heading (i-1)  -  heading (i)  >  pi 

nheading (i)  =  nheading (i-1)  +  heading (i)  -  heading (i-1)  +  2*pi 

end 
end 

heading  =  nheading'; 

%  MEASUREMENT  VECTOR 

y= [ug, vg, heading, yaw_rate, lat, long] ;  %  Complete  measured  data 
%  STATE  VECTOR 

x(:,s)=[lat(s) , long ( s ) , psiO , yaw_rate ( s ) *pi/l 80 , ug, vg, br , bpsi ] ' ; 
psiO=heading (startSample) ; 
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%  Initialize  the  state  vector,  x  is  8,  y  is  6 
x=zeros ( 8 , endSample) ;  err=zeros (6, endSample) ; 
s=s tart Sample; 

x  ( : ,  s )  =  [  lat  ( s )  ,  long  ( s )  ,  psiO ,  yaw_rate  ( s )  ,  ug  ( s )  ,  vg  ( s )  ,  Bias_r  ( s )  , 

Bias_psi  ( s ) ]  '  ; 

%  Initial  A  matrix 
A=zeros (8,8); 

X=x ( 1 , s ) ; Y=x (2 , s ) ; psi=x ( 3 , s ) ; r=x  ( 4 , s ) ;  dop_ug=x ( 5 , s ) ; dop_vg=x ( 6, s ) 
br=x ( 7 , s ) ; bpsi=x ( 8 , s ) ; 

A ( 1 , 3 ) =-dop_ug*sin (psi ) -dop_vg*cos (psi ) ; 

A ( 1 , 5 ) =cos (psi )  ; 

A(l,6)=-sin(psi) ; 

A (2 , 3 ) =dop_ug*cos (psi ) -dop_vg*sin (psi )  ; 

A (2 , 5 ) =sin (psi )  ; 

A  (2 , 6) =cos (psi ) ; 

A ( 3 , 4 ) =1 ; 

if  (gpsStatus ( 1 , 1 ) ==1 . 0  &  NsV(l)>3)  %  The  NsV>3  rejects  fixes  that  are 
less  than  4  satellites 
C=zeros (6,8); 

C  ( 1 , 5 )  =  1 ; 

C  ( 2 , 6 )  =  1 ; 

C(3,3)=1;C(3,8)=1; 

C (4, 4) =1;C (4, 7) =0;  %  C(4,7)  Forces  B_r  to  0 

C(5, 1) =1; 

C  (  6 ,  2  )  =  1 ; 

else 

C=zeros (6,8); 

C ( 1 , 5 ) =1 ; 

C (2, 6) =1; 

C(3,3)=1;C(3,8)=1; 

C ( 4 , 4 ) =1 ; C ( 4 , 7 ) =0 ; 

C ( 5 , 1 ) =0 ; 

C (6, 2) =0; 

end 

%%  System  Noise 


& 
h- 1 

II 

o 

o 

%variance 

on 

X, 

mA2 

q2=0 . 0 ; 

%variance 

on 

Y, 

mA2 

q3=0 . 001 ; 

%variance 

on 

psi 

,  radA 

2 

II 

o 

I—1 

%variance 

on 

r. 

rad/ s ) 

q5=0 . 01 ; 

%variance 

on 

ug. 

(m/s) A 

2 

q6=0 . 01 ; 

%variance 

on 

vg. 

(m/s) A 

2 

q7=0. 0000001;  % 
q8=0.0;  % 

Q_dummy= [ ql ; q2 ; q3 ; q4 ; q5 ; q6 ; q7 ; q8 ]  ; 
Q=diag (Q_dummy) ; 


nul 

=0.001; 

nu2 

=0.0001; 

nu3 

=0.001; 

nu4 

=0.001; 

nu5 

=  1.0; 

nu6 

=  1.0; 
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%%  Initial  CoVariance  Matrix 

%  Note,  old  after  means  measured  data  at  old  time,  new  before  means 
%  model  predicted  value 

p_old_af ter=eye ( 8 ) * le-2 ; 
delx_old_af ter=zeros (8,1); 
g=ones (8,1); 

psave=zeros ( 8 , startSample : endSample) ; 

%%  Main  Calculation  Loop 
for  i=startSample : endSample-1 

%  Measurement  Noise  -  Adjusts  the  values  of  the  R-matrix  based 
%  on  the  quality  of  the  fix  or  if  no  fix  present, 
if  NsV ( i ) <=3 
nu5=l . 0 ; 
nu6=l . 0 ; 

elseif  NsV(i)==4 
nu5=0 . 1 ; 
nu6=0 . 1 ; 

elseif  NsV(i)>=5 
nu5=0 .  01 ; 
nu6=0 . 01 ; 

end 

R  dummy= [nul ; nu2 ; nu3 ; nu4 ; nu5 ;  nu6]  ; 

R=diag(R  dummy); 

%  Compute  linearized  PHI  matrix  using  updated  A 
%  Phi=expm (A*dt) ; 

phi=eye (8,8) +A*dt+ (A*dt) A2/2+ (A*dt) A3/ 6; 
x_old_af ter=x ( : , i ) ;  %reset  initial  state 

% [x_new_before] =prop8 (x_old_af ter , 0, 0, dt) ;  %  nonlinear  state 

%  propagation 

xold=x  old  after; 

%X=xold(l) ;Y=xold(2) ;psi=xold(3) ;r=xold(4) ;ug=xold(5) ;vg=xold(6) ; 
%bu=xold(7) ; bpsi=xold ( 8 )  ; 

fl=xold (5) *cos (xold (3) ) -xold (6) *sin (xold (3) ) ; 
f2=xold (5)*sin(xold(3)) txold (6) *cos (xold (3) )  ; 
f 3=xold ( 4 ) ; 
f4=0;  %rdot=0; 
f5=0;  %ugdot=0; 
f6=0;  %vgdot=0; 
f  =  [  f  1 ;  f  2 ;  f  3 ;  f  4 ;  f  5 ;  f  6 ;  0 ;  0  ]  ; 
xnew=xold+f . *dt;  %xd=f; 
x  new  before=xnew; 

p  new  before=phi*p  old  after*phi '  +  Q;  %  error  covariance 

%  propagation 

%  new  gain  calculation  using  linearized  new  C  matrix  and  current  state 
%  error  covariances. 

%  formulate  the  innovation  using  nonlinear  output  propagation  as 
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%  compared  to  new  sampled  data  from  measurements 

if  (gpsStatus ( i+1 , 1 ) ==1 . 0  &  NsV(i+l)>3) 
ny_sub=6; 

xold=x  new  before; 

yhat= [xold (5) ;xold (6) ;xold (3) +xold (8) ;xold (4) +xold (7) ;xold (1) ;xold (2) ] 
else 

ny_sub=4 ; 

xold=x  new  before; 

yhat= [xold (5) ; xold (6) ; xold (3) +xold (8) ; xold (4) +xold (7) ; 0; 0] ; 

end 

err ( : , i+1) = (y (i+1, 1 : 6) '  -  yhat) ;  %  Innovation 

if  (gpsStatus ( i+1 , 1 ) ==1 . 0  &  NsV ( i+1 , 1 ) >3 ) 

C=zeros (6,8) ; 

C  ( 1 , 5 )  =  1 ; 

C (2 , 6) =1; 

C(3,3)=1;C(3, 8)=1; 

C ( 4 , 4 ) =1 ; C ( 4 , 7 ) =0 ; 

C(5,l)=l; 

C (6, 2) =1; 

else 

C=zeros (6,8)  ; 

C  ( 1 , 5 )  =  1 ; 

C(2, 6)=1; 

C(3,3)=1;C(3, 8)=1; 

C ( 4 , 4 ) =1 ; C ( 4 , 7 ) =0 ; 

C ( 5 , 1 ) =0 ; 

C (6, 2) =0; 

end 

if  ny_sub==6, 
if 

(  (y  (i  +  1,  1)  ==y  (i,  1))  I  I  (y  (i+1, 1)  ==0 . 0)  |  (abs  (y  (i  +  1,  1)  )  >=5 . 0)  )  ,  C  (1,  : )  =... 

0 . 0*g ' ; end; 
if 

(  (y  (i  +  1, 2)  ==y  (i,  2)  )  |  |  (y  (i+1, 2)  ==0 . 0)  |  (abs  (y  (i  +  1, 2)  )  >=5 . 0)  )  ,  C  (2,  : )  =... 

0 . 0*g ' ; end; 

if  (abs(y(i+l,3)-y(i,3) ) <=0.000001) ,C(3, : ) =0 . 0*g ' ; end; 
if  (abs(y(i+l,4)-y(i,4) ) <=0.000001) ,C(4, : ) =0 . 0*g ' ; end; 
if  (abs(y(i+l,5)-y(i,5) ) <=0.000001) ,C(5, : ) =0 . 0*g ' ; end; 
if  (abs(y(i+l,6)-y(i,6) ) <=0.000001) ,C(6, : ) =0 . 0*g ' ; end; 

else 

if 

(  (y  (i  +  1,  1)  ==y  (i,  1))  I  I  (y  (i+1, 1)  ==0 . 0)  |  (abs  (y  (i  +  1, 1)  )  >=5 . 0)  )  ,  C  (1,  : )  =... 

0 . 0*g ' ; end; 
if 

(  (y  (i  +  1, 2)  ==y  (i,  2))  |  |  (y  (i+1, 2)  ==0 . 0)  |  (abs  (y  (i  +  1, 2)  )  >=5 . 0)  )  ,  C  (2,  : )  =... 

0 . 0*g ' ; end; 

if  (abs(y(i+l,3)-y(i, 3) )<=0. 000001), C(3, : ) =0 . 0*g ' ; end; 
if  (abs(y(i+l,4)-y(i,4) ) <=0.000001) ,C(4, : ) =0 . 0*g ' ; end; 

end 


64 


%  Compute  gain,  update  Total  State  and  error  covariance 

G=p  new  bef ore*C  ( 1 :  ny  sub,  : )  '  *inv  (C  ( 1 :  ny  sub,  : )  *p  new  before*... 

C ( 1 : ny  sub, : ) '  +  R ( 1 : ny  sub, 1 : ny  sub) ) ;  %  Kalman  Gain 

p_temp=G*C (1 :ny_sub, : ) *p_new_before; 
p  old  after=p  new  before-p  temp; 

psave ( : , i+1 ) =diag (p_old_af ter ) ; 

x  new  after=x  new  before  +  G*err(l:ny  sub, i+1); 


%i 

%endSample 

%carry  new  state  into  next  update 
x(:,i+l)=x  new  after; 

%resetting  up  the  linearized  A  matrix 
A=zeros (8,8); 

X=x ( 1 , i  +  1 ) ; Y=x (2 , i  +  1 ) ; psi=x ( 3 , i  +  1 ) ; r=x ( 4  ,  i  +  1 ) ; 

dop_ug=x (5, i  +  1) ;dop_vg=x (6, i  +  1) ;bu=x (7, i  +  1) ;bpsi=x (8,  i  +  1) ; 

A(l, 3) =-dop_ug*sin (psi) -dop_vg*cos (psi) ; 

A  ( 1 , 5 ) =cos (psi ) ; 

A(l,6)=-sin(psi)  ; 

A(2, 3) =dop_ug*cos (psi) -dop_vg*sin (psi) ; 

A(2, 5) =sin (psi) ; 

A(2, 6) =cos (psi)  ; 

A  ( 3 , 4  )  =  1 ; 

end; 

%%  Plot  Results  in  Figures 
%  figure  ( 1 ) 

O, 

o 

plot (long ( start Sample : ends ample) , lat ( start Sample : ends ample) , ' r+ ' ) , grid 
%  hold  on 

%  plot (x (2 , startSample : endSample) , x ( 1 , startSample : endSample) , ' g . ' ) 

%  hold  off 

%  title (' Filter  Estimated  Path (green),  GPS  (red)') 

%  ylabel ( ' latitude  in  meters ' ) 

%  xlabel ( ' longitude  in  meters ' ) 

%  grid 

%  axis  equal 

figure (2)  %  Path  Plot 

%plot (long, lat, ' r+ ' ) ; 

%hold  on 

plot (x (2, : ) , x (1, : ) , ' g- ' ) ; 
hold  on 

plot (d_khs ( : , 12 ) -d_khs ( s , 12 ) , d_khs ( : , 1 1 ) -d_khs ( s , 1 1 ) , ' b- ' ) ; 
plot (GPS_3 ( : , 2) -d_khs (s, 12) , GPS_3 ( : , 1 ) -d_khs (s, 11 ) , ' *k', . . . 

GPS  4 ( : , 2 ) -d  khs ( s , 12 ) , GPS  4(:,l)-d  khs  (s, 11) ,  ' *m' , . . . 


65 


GPS_5 ( : , 2 ) -d_khs ( s , 12 ) , GPS_5 ( : , 1 ) -d_khs ( s , 1 1 ) , ' *r ' ) ; 
hold  off 
grid 

title (' POSITION :  GPS,  Filter  Estimated,  In-Vehicle  Estimated'); 
legend  (' Filter Vehicle  Filter',  'GPS  NsV=3','GPS  NsV=4',... 

'GPS  NsV=5 ' ) ; 

ylabel ( ' X  [m] ' ) 
xlabel ( ' Y  [m] ' ) 
axis  equal 

%  figure (3)  %  X  Dimension 

%  plot ( t ( start Sample : ends ample) , lat ( start Sample : ends ample) , ' r+ ' , 

%  t ( start Sample : ends ample) , x ( 1 , start Sample : ends ample)  ,  ' g . '  , 

%  t ( startSample : endSample) , d  khs ( startSample : endSample,  1 1 ) - 

%  d_khs (s, 11) , 'b. ' ) ; 

%  title ('X:  measured  [red],  sim  estimated  [green],  exp  estimated 

%  [blue] ' ) ; 

o, 

o 

%  figure (4)  %  Y  Dimension 

O, 

o 

%  plot ( t (startSample : endSample) , long (startSample : endSample) , ' r+ ' , . . . 
%  t (startSample : endSample) , x (2 , startSample : endSample) ,  ' g . '  , 

%  t ( startSample : endSample) , d  khs ( startSample : endSample, 12 ) - 

%  d_khs (s, 12) , 'b. ' ) ; 

%  title ('Y:  measured  [red],  sim  estimated  [green],  exp  estimated 

%  [blue]  '  )  ; 

figure  (5)  %  Heading  Results 

plot ( t (startSample : endSample) , heading (startSample : endSample) , ' r+ ' , . . 

t (startSample : endSample) , (x (3, startSample : endSample) ) , ' g . ' , 

t (startSample : endSample) , (d_khs (startSample : endSample, 13 ) ) , ' b . ' ) ; 

title (' Heading  (radians):  measured  [red],  sim  estimated  [green],, 
exp  estimated  [blue]  '  )  ; 

figure  (6)  %  Yaw  Rate 

plot (t (startSample : endSample) , yaw  rate (startSample : endSample) , ' r+ ' , . 

t (startSample : endSample) , x ( 4 , startSample : endSample) , ' g . ' , . . 

t (startSample : endSample) , d  khs (startSample : endSample, 14 ) , ' b . ' ) ; 

title  (' Yaw_rate :  measured  [red],  sim  estimated  [green],  exp... 
estimated  [blue]  '  )  ; 

figure  (7)  %  Speed,  ug 

plot ( t (startSample : endSample) , ug (startSample : endSample) , ' r+ ' , . . . 
t (startSample : endSample) , x ( 5 , startSample : endSample) , ' g . ' , . . 

t (startSample : endSample) , d_khs (startSample : endSample, 15 ) , ' b . ' ) ; 

title  ('ug:  measured  [red],  sim  estimated  [green],  exp  estimated... 
[blue]  '  )  ; 

figure  (8)  %  Speed,  vg 


66 


plot ( t ( start Sample : ends ample) , vg  RDI ( start Sample : ends ample) , ' r+ ' , . . . 

t ( start Sample : ends ample) , x ( 6, start Sample : ends ample) , ' g . ' , . . 

t ( start Sample : ends ample) , d_khs ( start Sample : ends ample, 16)  ,  ' b .  '  )  ; 

title  ('vg:  measured  [red],  sim  estimated  [green],  exp  estimated... 
[blue]  '  )  ; 

figure (9)  %  Bias_r 

plot ( t ( start Sample : ends ample) , x ( 7 , start Sample : ends ample) , ' g . ' , . . 
t ( start Sample : ends ample) , Bias  r ( start Sample : ends ample, 1 ) , ' b 

grid 

title (' Bias_r :  sim  estimated  [green],  exp  estimated  [blue]'); 
xlabel('time  in  seconds') 
ylabel ( '  [deg/ sec]  '  ) 

figure (10)  %  Bias_psi 

plot ( t ( start Sample : ends ample) , x ( 8 , start Sample : ends ample) , ' g . ' , . . 

t ( start Sample : ends ample) , Bias  psi ( start Sample : ends ample, 1 ) , ' b . ' ) 
grid 

title (' Bias_\psi :  sim  estimated  [green],  exp  estimated  [blue]'); 
xlabel('time  in  seconds') 
ylabel (' B_\psi  (rad) ') 
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APPENDIX  C:  SIMULATION  UKF  CODE 


The  following  MATLAB  code  was  used  for  running  simulations  on  the  data  sets 
to  evaluate  UKF  performance  with  EKF  performance.  The  original  code  was  developed 
by  H.S.  Kim  based  on  Julier  and  I Jhlmann’s  algorithm  (1997)  with  modifications  made 
by  S.  Vonheeder  during  the  course  of  this  work. 


2-9-  2'9'9'2'9'9'9'2'2'9'2'9'2'9'9'9'9'9'2'9'9'9'2'9'2'9'2'9'2'9'2'9'2'9'2'9'9'9'9'2'9'2'9'2'9'9'9'9'2'9'2'9'2'9'2'9'9'9'2'2'9'9'9'2'2'9'2' 

oo  ooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo 

%  AUV  Research  Center  % 

%  Unscented  Kalman  Filter  For  ARIES  AUV  % 

%  Edited:  11/09/06  % 

S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S-S- 

oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo 

close  all;  clear  all;  clc 


%%  Load  data  from  Desired  Run  with  Aries  using  d  and  NavD  Files 
%  generate  default  file  name  for  dialog  box 
def DateFields  =  datevec (date)  ; 

defYear  =  sprintf (' %02d def DateFields ( 1 )  -  2000); 
defMon  =  sprintf (' %02d def DateFields (2 ))  ; 
defDay  =  sprintf (' %02d def DateFields ( 3 )) ; 
defDateStamp  =  [defMon  defDay  defYear  '  Ol.d']; 

defNameD  =  [ ' d '  defDateStamp]; 


%  get  d-file  name  from  dialog  box 
fields  =  { ' D  File:'}; 
boxTitle  =  'Get  Data  File'; 
lineNo  =  1; 

defaultlnput  =  {defNameD}; 

input  =  inputdlg ( fields ,  boxTitle,  lineNo,  defaultlnput,  'on'); 
if  (  isempty (input)  ) 

disp('No  data  file  selected.  Exit  program.'); 
return; 

else 

dFileName  =  input{l}; 

end; 


%  get  NavD-file  name  from  dialog  box 
fields  =  {'NavD  File:'}; 
boxTitle  =  'Get  Nav  Data'; 
lineNo  =  1; 

defNameNavD  =  ['NavD  '  dFileName (2 : 10)  ' . d ' ] ; 

defaultlnput  =  {defNameNavD}; 

input  =  inputdlg ( fields ,  boxTitle,  lineNo,  defaultlnput,  'on'); 
if  (  isempty (input)  ) 

disp('No  data  file  selected.  Exit  program.'); 
return; 

else 

dFileNameNav  =  input{l}; 

end; 
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d  =load (dFileName) ; 

Nav  =load (dFileNameNav) ; 


o,  o, _ 

Q  Q - 

%  Data  Manipulation  from  the  experiment  NAV  data 

O, _ 

O - 

mmm=max (size (Nav) ) ; 
startSample=l ; 
endSample=mmm; 


d  khs  ( : 

,D 

=Nav 

( : , 21)  ; 

%  gpsStatus  check  ! ->  ok) 

d  khs  ( : 

,2) 

=Nav 

( : , 32)  ; 

%  IMU 

theta  [rad] 

d  khs  ( : 

,3) 

=Nav 

< : , 31)  ; 

%  IMU 

phi  [rad] 

d  khs  ( : 

,4) 

=Nav 

( : , 54)  ; 

%  IMU 

psi  [rad] 

d  khs ( : 

,5) 

=Nav 

( : ,  8 )  ; 

%  yaw 

rate  [rad/sec] 

d  khs  ( : 

,6) 

=Nav 

( : ,  10)  ; 

%  u  [ 

m/ sec] 

d  khs  ( : 

,7) 

=Nav 

( : , 11)  ; 

%  v  [ 

m/ sec] 

d  khs  ( : 

,8) 

=Nav 

< : , 12)  ; 

%  w  [ 

m/ sec] 

d  khs  ( : 

,9) 

=Nav 

( : , 47)  ; 

%  GPS 

longitude 

d  khs ( : 

,  10) 

=Nav 

( : ,  46)  ; 

%  GPS 

latitude 

d  khs  ( : 

,11) 

=Nav 

( : , 35)  ; 

%  Nav 

lat  (X  [m]  ) 

d  khs  ( : 

,  12) 

=Nav 

( : ,  36)  ; 

%  Nav 

long  (Y  [m]  ) 

d  khs  ( : 

,13) 

=Nav 

( : ,  37)  ; 

%  Nav 

heading  [rad] 

d  khs  ( : 

,14) 

=Nav 

( : ,  38)  ; 

%  Nav 

yaw  rate  [rad/sec] 

d  khs  ( : 

,  15) 

=Nav 

( : ,  39)  ; 

%  Nav 

u  [m/sec] 

d  khs  ( : 

,  16) 

=Nav 

( : ,  40)  ; 

%  Nav 

v  [m/sec] 

d  khs  ( : 

,17) 

=Nav 

< : , 42)  ; 

%  Nav 

Bias  r  [rad/sec] (exp  est  Bias 

d  khs  ( : 

,18) 

=Nav 

(  :  , 41)  ; 

%  Nav 

Bias  psi  [rad]  (exp  ested  Bias 

Bias  ps 

i 

=Nav 

(  :  , 41)  ; 

%  [rad]  (exp  est  Bias  psi) 

Bias  r 

o,  o. 

=Nav 

( : , 42)  ; 

%  [rad/sec]  (exp  est  Bias  r) 

"o  "o 

%  Initi 

o. 

alize  the  experimental  result 

s 

o 

gpsStatus 

=d 

khs ( : , 1) ; 

pitch 

=d 

khs ( : , 2) ; 

roll 

=d 

khs ( : , 3) ; 

heading 

=d 

khs ( : , 4) ; 

yaw  rate 

=d 

khs ( : , 5) ; 

ug 

=d 

khs ( : , 6) ; 

vg 

=d 

khs ( :  ,  7) -1 

0*d  khs ( : , 

5)  ; 

wg 

=d_ 

khs ( : , 8) ; 

long 

=d 

khs ( : , 9) ; 

lat 

=d 

khs ( : ,  10) ; 

ll=long (startSample) ; 

12=lat (startSample)  ; 

long=long-ll . *ones (length (lat)  ,  1)  ; 

lat=lat-12*ones (length (lat)  ,  1)  ; 

for  i=l : mmm, 

if  (gpsStatus (i, 1 ) ==0) 

long ( i , 1 ) =0 . 0 ; lat ( i , 1 ) =0 . 0 ; 
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end 


else 

end 


o, _ 

Q - 

%  Time  Vector 

O, _ 

Q - 


dt=l / 8 ; 

t=0 : dt : ( length (ug) -1 ) *dt ; 


o, _ 

O - 

%  Heading  Signal  Wrapping 

Q, _ 

O - 

nheading  =  zeros (1,  length (heading) ) ; 
nheading(l)  =  heading (1); 

for  i=2 : length (heading) 

if  abs (heading ( i )  -  heading (i-1) )  <=  pi 

nheading (i)  =  nheading (i-1)  +  heading (i)  -  heading (i-1) ; 

end 

if  heading (i)  -  heading (i-1)  >  pi 

nheading (i)  =  nheading (i-1)  +  heading (i)  -  heading (i-1)  -  2*pi; 

end 

if  heading (i-1)  -  heading (i)  >  pi 

nheading (i)  =  nheading (i-1)  +  heading (i)  -  heading (i-1)  +  2*pi; 

end 
end 

heading  =  nheading'; 


%  MEASUREMENT  VECTOR 


O _ 

O - 

y= [ug, vg, heading, yaw_rate, lat, long] ;  %complete  measured  data 


0,0, _ 

O  O - 

%  Initialize  the  UKF  parameters  and  state  vector,  x  is  8,  y  is  6 

O _ 

O - 

nx=8 ; 
ny=6; 
nv=  8  ; 
nn=6; 
na=nx; 

scale=3 ; 
kappa=scale-na; 

WO  m=kappa/ (na+kappa) ; 

Wi  m=l/ (2* (na+kappa) )  ; 

WO_c=kappa/ (na+kappa) ; 

Wi_c=l/ (2* (na+kappa)  )  ; 

s=s t ar t Sample; 

x=zeros (8, endSample) ;  err=zeros (6, endSample) ; 
psiO=heading (s); 

x ( ; ,  s )  =  [lat ( s )  ,  long ( s ) , psiO , yaw_rate ( s ) , ug ( s ) , vg ( s ) ,  .  .  . 
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Bias_r ( s ) , Bias_psi ( s ) ] ' ; 

xa ( : , s )  =  [ x ( : , s ) ]  ; 
xa_old=xa (  : ,  s )  ; 
x_old=x ( : ,  s )  ; 

%%  System  Noise 


ql=0 . 01 ; 

%variance 

on 

X 

> 

M 

Cannot  be  0, 

Q  pos  semi-def 

q2=0 . 01 ; 

%variance 

on 

Y,  mA2 

for  Cholesky 

Decomposition 

q3=0 . 001 ; 

%variance 

on 

psi,  rad 

A2 

q4=0 . 1 ; 

%variance 

on 

r ,  ( rad/ s ) A2 

q5=0 . 01 ; 

%variance 

on 

ug, (m/s) 

A2 

q6=0 .01; 

%variance 

on 

vg, (m/ s) 

A2 

q7=0. 0000001; 

%variance 

on 

B  r 

CO 

II 

o 

o 

%variance 

on 

B  psi 

Q_dummy= [ ql ; q2 ; q3 ; q4 ; q5 ; q6 ; q7 ; q8 ] ; 
Q=diag (Q_dummy) ; 

%  Measurement  Noise 

nul=0 . 1 ; 

nu2=0 . 1 ; 

nu3=0 . 001 ; 

nu4=0 . 001; 

nu5=l . 0 ; 

nu6=l . 0 ; 

R  dummy= [nul;nu2;nu3;nu4;nu5;nu6] ; 
R=diag(R  dummy); 


g,  g, _ 

o  o - 

%  Initial  Pa  matrix 

O _ 

O - 

p_old_af ter=eye (nx) *le-2; 

delx  old  after=zeros (nx, 1) ; 
g=ones (nx, 1 )  ; 

psave=zeros (nx, startSample : endSample) ; 

Pa_o 1 d=p_o 1 d_a  f t e  r ; 

%%  Main  Calculation  Loop 
for  i=startSample : endSample-1 
%i 

%endSample 

o _ 

O - 

%  1.  Calculate  Sigma  points 

o _ 

O - 

bias_Pa=chol ( (na+kappa) *Pa_old) ; 


o _ 

O - 

Sigma_points_old ( : , 1 ) =xa_old; 
for  j=l:na, 

Sigma_points_old ( : , j  +1 ) =xa_old+bias_Pa ( j ,  : )  ' ; 
Sigma_points_old ( : , na+ j  +1 ) =xa_old-bias_Pa ( j ,  : )  ' ; 

end 
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o _ 

O - 

%  Time  Update 

o _ 

O - 

%  2.  Nonlinear  state  propagation 

O, _ 

o 

for  j  = 1 :  (2*na+l) 

%  Sigma  points  new  bef ore ( 1 : nx, j ) =prop  ukf... 

%  ( Sigma_points_old (1 :nx, j ) , (process_noise*0 . 0) , dt) ; 

xold=Sigma  points  old(l:nx,j); 

fl=xold (5) *cos (xold (3) ) -xold (6) *sin (xold (3) ) ; 

f2=xold (5)*sin(xold(3)) +xold (6) *cos (xold (3) )  ; 

f 3=xold ( 4 ) ; 

f4=0;  %rdot=0; 

f5=0;  %ugdot=0; 

f6=0;  %vgdot=0; 

f7=0;  %Bias_r_dot=0 ; 

f8=0;  %Bias_psi_dot=0; 

f  =  [  f  1 ;  f  2 ;  f  3 ;  f  4 ;  f  5 ;  f  6 ;  f  7 ;  f  8  ]  ; 

xnew=xold+f . *dt; 

Sigma  points  new  bef ore ( 1 : nx, j ) =xnew; 

end 

O, _ 

o 

%  3.  mean  value  of  x  new  before 

o, _ 

o 

mean  x  new  bef ore=zeros (nx, 1 ) ; 
for  j  =1 :  (2*na+l) 
if  j==l 

mean  x  new  before=mean  x  new  before+WO  m* . . . 

Sigma  points  new  bef ore ( 1 : nx, j ) ; 

else 

mean  x  new  before=mean  x  new  before+Wi  m* . . . 

Sigma_points  new  bef ore ( 1 : nx, j ) ; 

end 

mean  x  new  before (7) =0;  %  B  r=0 

end 

O, _ 

o 

%  4 .  error  covariance  propagation 

O, _ 

o 

P  new  before=Q; 
for  j=l : (2*na+l) , 
if  j==l 

P  new  before=P  new  before+W0_c* . . . 

(Sigma  points  new  bef ore ( 1 : nx, j ) -mean  x  new  before)*... 
(Sigma  points  new  bef ore ( 1 : nx, j ) -mean  x  new  before) 

else 

P  new  before=P  new  before+Wi  c* . . . 

( Sigma_points  new  bef ore ( 1 : nx, j ) -mean  x  new  before)*... 
( Sigma_points  new  bef ore ( 1 : nx, j ) -mean  x  new  before) 

end 

end 

O, _ 

o 

%  5.  output  Sigma_yhat 

o, _ 

o 

for  j=l : (2*na+l) , 
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if  gpsStatus ( i+1 ) ==1 
ny_sub=ny; 

%  Sigma  yhat  before ( 1 : ny, j ) =output  ukf  w  GPS... 

%(Sigma  points  new  before ( 1 : nx, j ), (measurement  noise*0.0)); 

xold ( 1 : nx) =Sigma  points  new  bef ore ( 1 : nx, j ) ; 

yl=xold (5 ) ; 

y2=xold ( 6)  ; 

y3=xold ( 3 ) +xold ( 8 ) ; 

y4=xold (4) +xold (7) *0; 

y5=xold ( 1 ) ; 

y6=xold  (2 ) ; 

yhat=[yl;y2;y3;y4;y5;y6] ; 

Sigma_yhat_bef ore ( 1 : ny, j ) =yhat; 

else 

ny_sub=ny-2 ; 

%  Sigma  yhat  bef ore ( 1 : ny, j ) =output  ukf  wo  GPS... 

% (Sigma  points  new  bef ore ( 1 : nx, j ), (measurement  noise*0.0)); 

xold ( 1 : nx) =Sigma  points  new  bef ore ( 1 : nx, j ) ; 

yl=xold ( 5 ) ; 

y2=xold ( 6) ; 

y3=xold ( 3 ) +xold ( 8 ) ; 

y4=xold (4) +xold (7) *0; 

y5=xold ( 1 ) ; 

y6=xold (2 ) ; 

yhat= ( yl ; y2 ; y3 ; y4 ; 0 ; 0 ] ; 

Sigma_yhat_bef ore ( 1 : ny, j ) =yhat; 

end 

end 

o, _ 

o 

%  6.  mean  value  of  yhat 

o, _ 

o 

mean_yhat=zeros (ny,  1) ; 
for  j=l : (2*na+l) , 
if  j==l 

mean  yhat=mean  yhat+WO  m*Sigma  yhat  bef ore ( 1 : ny, j ) ; 

else 

mean  yhat=mean  yhat+Wi  m*Sigma  yhat  bef ore ( 1 : ny, j ) ; 

end 

end 

O _ 

O - 

%  Measurement  Update 

o _ 

O - 

o, _ 

o 

%  1 .  Pyy  bar  and  Pxy 

o, _ 

o 

Pyy_bar=zeros (ny_sub, ny_sub) ; 

Pxy=zeros (nx, ny_sub) ; 

for  j=l : (2*na+l) , 

err  x=Sigma  points  new  bef ore ( 1 : nx, j ) -mean  x  new  before; 
err_y=Sigma_yhat_bef ore ( 1 : ny_sub, j ) -mean^yhat ( 1 : ny__sub, 1 ) ; 
if  j==l 

Pxy=Pxy+W0  c*err  x*err  y'; 
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Pyy  bar=Pyy  bar+WO  c*err  y*err  y'; 

else 

Pxy=Pxy+Wi  c*err  x*err  y'; 

Pyy  bar=Pyy  bar+Wi_c*err  y*err  y'; 

end 

end 

S_temp=Pyy_bar-R ( 1 : ny_sub, 1 : ny_sub) ; 
if  gpsStatus ( i+1 ) ==1 

if  ( (y(i  +  l, l)==y(i, 1) )  I  I  (y (i+1, 1)==0.0)  I  I  . . . 

(abs(y(i+l,l) )>=5.0) ) , Pxy ( : , 1) =0 . 0*g 
S_temp ( : , 1) =0 . 0*g (1 : ny_sub, 1 ) ; 

S_temp ( 1 , : ) =0 . 0*g (1 : ny_sub, 1 ) ' ; 

end 

if  ( (y(i  +  l,2)==y(i,2) )  I  I  (y (i  +  1, 2) ==0.0)  I  I  .  .  . 

(abs(y(i+l,2) )>=5.0) ) , Pxy ( : , 2) =0 . 0*g 
S_temp ( : , 2 ) =0 . 0*g (1 : ny_sub, 1 ) ; 

S_temp (2 , : ) =0 . 0*g (1 : ny_sub, 1 ) ' ; 

end 

if  (abs(y(i+l,3)-y(i,3) ) <=0.000001) , Pxy (:,3)=0.0*g; 
S_temp ( : , 3) =0 . 0*g (1 :ny_sub, 1) ; 

S_temp (3, : ) =0 . 0*g (1 :ny_sub, 1) ' ; 

end 

if  (abs(y(i+l,4)-y(i,4) )<= 0.000001) , Pxy (:,4)=0.0*g; 
S_temp ( : , 4 ) =0 . 0*g ( 1 : ny_sub, 1 ) ; 

S_temp (4, : ) =0 . 0*g (1 :ny_sub, 1) ' ; 

end 

if  (abs(y(i+l,5)-y(i,5) ) <=0.000001) , Pxy (:,5)=0.0*g; 
S_temp ( : , 5) =0 . 0*g (1 :ny_sub, 1) ; 

S_temp (5, : ) =0 . 0*g (1 :ny_sub, 1) ' ; 

end 

if  (abs (y (i+1, 6) -y (i, 6) ) <=0.000001) , Pxy (:,6)=0.0*g; 
S_temp ( : , 6) =0 . 0*g ( 1 : ny_sub, 1 ) ; 

S_temp (6, : ) =0 . 0*g (1 : ny_sub, 1)  '  ; 

end 

else 

if  ( (y (i  +  1, 1) ==y (i, 1))  I  I  (y (i+1, 1) ==0 .0)  |  |  . . . 

(abs(y(i+l,l) ) >=5 . 0) ) , Pxy ( : , 1) =0 . 0*g 
S_temp ( : , 1) =0 . 0*g (1 : ny_sub, 1 ) ; 

S_temp ( 1 , : ) =0 . 0*g (1 : ny_sub, 1 ) ' ; 

end; 

if  ( (y(i  +  l,2)==y(i,2) )  I  I  (y (i  +  1, 2) ==0.0)  I  I  .  .  . 

(abs(y(i+l,2) ) >=5 . 0) ) , Pxy ( : , 2) =0 . 0*g 
S_temp ( : , 2 ) =0 . 0*g (1 : ny_sub, 1 ) ; 

S_temp (2 , : ) =0 . 0*g (1 : ny_sub, 1 ) ' ; 

end; 

if  (abs(y(i+l,3)-y(i,3) ) <=0.000001) , Pxy (:,3)=0.0*g; 
S_temp ( : , 3) =0 . 0*g (1 :ny_sub, 1) ; 

S_temp (3, : ) =0 . 0*g (1 :ny_sub, 1) ' ; 

end; 

if  (abs(y(i+l,4)-y(i,4) ) <=0.000001) , Pxy (:,4)=0.0*g; 
S_temp ( : , 4 ) =0 . 0*g ( 1 : ny_sub, 1 ) ; 

S_temp ( 4 , : ) =0 . 0*g ( 1 : ny_sub, 1 ) ' ; 

end; 
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end 


o, _ 

o 

%  2 .  UKF  Gain  matrix 

O, _ 

o 

UKF  K=Pxy*inv ( S_temp+R ( 1 : ny  sub,l:ny  sub)); 

O, _ 

o 

%  3 .  x  new 
%  3 .  x  new 

O, _ 

o 

err ( : , i+1) = (y (i+1, : ) ' -mean^yhat) ; 

O, _ 

o 

if  sqrt (err (5, i+1) A2+err(6,i+l) A2) >100, 
err(5,i+l)=0;err(6,i+l)=0; 
end; 

o, _ 

o 

x  new=mean  x  new  before+UKF  K*err(l:ny  sub, i+1); 
x  new ( 7 ) =0 ; 

O, _ 

o 

%  4 .  P  new 

O, _ 

o 

P  new=P  new  before-UKF  K*(Pyy  bar) *UKF  K'; 

O, _ 

o 

%  5.  xa_old  &  Pa_old 

o, _ 

o 

xa  old=x  new; 

Pa_old=P  new; 

psave ( : , i  +  1 ) =diag ( P_new)  ; 

x ( : , i+1 ) — x  new; 

end 


%%  Plot  Results  in  Figures 
figure ( 1 ) , elf 

plot (long ( start Sample : ends ample) , lat ( start Sample : ends ample) , ' r . ' ) 

grid 

hold  on 

plot (x (2 ,  startSample : endSample) , x ( 1 , startSample : endSample) ,  ' g- ' ) 
hold  off 

title (' Filter  Estimated  Path (green) , GPS  (red)') 
ylabel ( ' latitude  in  meters ' ) 
xlabel ( ' longitude  in  meters ' ) 
grid 

axis  equal 

figure (2),  elf  %  Path  Plot 
plot (long, lat, ' r . ' ) ; 
hold  on 

plot (x (2, : ) , x (1, : ) , ' g- ' ) ; 

plot (d_khs ( : , 12 ) -d_khs ( s , 12 ) , d_khs ( : , 1 1 ) -d_khs ( s , 1 1 ) , ' b- ' ) ; 

hold  off 

grid 

legend ('GPS  Data',  'UKF  Estimate',  ' EKF  Estimate'); 

title ( s treat (dFileName (1:7) ,  ' \ _ ' , dFileName (9:12) ,  '  Path  Plot ' ) )  ; 

%  title (' POSTION :  GPS  [red],  UKF  Estimated  [green]  ,  In-Vehicle 
%  EKF  Estimated  [blue]  ' )  ; 
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ylabel ( ' X  [m] ' ) 
xlabel ( ' Y  [m] ' ) 
axis  equal 

figure (3),  elf  %  X  Dimension 

plot ( t ( start Sample : ends ample) , lat ( start Sample : ends ample) , ' r . ' , . . . 
t ( start Sample : ends ample) , x ( 1 , start Sample : ends ample) , ' g- ' , . . . 
t  ( start  Sample  :  ends  ample)  ,  d  khs  ( start  Sample  :  ends  ample,  11)... 
-d_khs (s, 11) , 'b- ' ) ; 

title  ('X:  measured  [red],  sim  estimated  [green],... 
exp  estimated  [blue] ' ) ; 

figure (4),  elf  %  Y  Dimension 

plot ( t ( start Sample : ends ample) , long ( start Sample : ends ample) , ' r . ' , . . . 
t ( start Sample : ends ample) , x (2 , start Sample : ends ample) , ' g- ' , . . . 
t  ( start  Sample  :  ends  ample)  ,  d  khs  ( start  Sample  :  ends  ample,  12 ) ... 
-d_khs (s, 12) , 'b- ' ) ; 

title  ('Y:  measured  [red],  sim  estimated  [green],... 
exp  estimated  [blue] ' ) ; 

figure (5),  elf  %  Heading  Results 

plot ( t ( start Sample : ends ample) , heading ( start Sample : ends ample)  ,  ' r . ' ,  . .  . 

t (startSample : endSample) , (x (3, startSample : endSample) ) , ' g- ' , . . . 
t (startSample : endSample) , (d_khs (startSample : endSample, 13 ) ) , ' b- 

'); 

title (' Heading  (radians):  measured  [red],  sim  estimated  [green], 
exp  estimated  [blue] ' ) ; 

figure (6),  elf  %  Yaw  Rate 

plot ( t (startSample : endSample) , yaw_rate (startSample : endSample) , ' r . ' , . . . 

t (startSample : endSample) , x ( 4 , startSample : endSample)  ,  ' g- ' ,  . .  . 
t  (startSample  :  endSample)  ,  d  khs  (startSample  :  endSample,  14 )  , ... 

'  b-  '  )  ; 

title  ('Yaw  rate:  measured  [red],  sim  estimated  [green],... 
exp  estimated  [blue] ' ) ; 

figure (7),  elf  %  Speed,  ug 

plot ( t (startSample : endSample) , ug (startSample : endSample)  ,  ' r . ' ,  . . . 
t (startSample : endSample) , x ( 5 , startSample : endSample) , ' g . ' , . . . 

t (startSample : endSample) , d_khs (startSample : endSample, 15 ) , ' b . ' ) ; 
title  ('ug:  measured  [red],  sim  estimated  [green],... 
exp  estimated  [blue] ' ) ; 

figure (8),  elf  %  Speed,  vg 

plot ( t (startSample : endSample) , vg (startSample : endSample)  ,  ' r . ' ,  .  .  . 
t (startSample : endSample) , x ( 6, startSample : endSample) , ' g . ' , . . . 
t (startSample : endSample) , d_khs (startSample : endSample, 1 6) , ' b . ' ) ; 
title  ('vg:  measured  [red],  sim  estimated  [green],... 
exp  estimated  [blue] ' ) ; 

figure (9),  elf  %  Bias_r 

plot ( t (startSample : endSample) , x ( 7 , startSample : endSample)  ,  ' g . ' ,  .  .  . 
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t ( start Sample : ends ample) , Bias  r ( start Sample : ends ample, 1 ) , ' b . ' ) 

grid 

title (' Bias_r :  sim  estimated  [green],  exp  estimated  [blue]'); 
xlabel('time  in  seconds') 
ylabel ( '  [deg/ sec]  '  ) 

figure (10),  elf  %  Bias_psi 

plot ( t ( start Sample : ends ample) , x ( 8 , start Sample : ends ample) , ' g . ' , . . . 

t ( start Sample : ends ample) , Bias  psi ( start Sample : ends ample, 1 ) , ' b . ' ) 
grid 

title (' Bias_\psi :  sim  estimated  [green],  exp  estimated  [blue]'); 
xlabel('time  in  seconds') 
ylabel (' B_\psi  (rad) ') 
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APPENDIX  D:  VEHICLE  IMU  CODE 


The  following  provides  the  IMU  code,  written  in  C,  that  processes  the  IMU  data 
output  and  perfonns  the  necessary  reference  frame  rotations  as  well  as  the  integration  of 
the  yaw  rate  for  the  heading  reference.  The  accelerations,  angular  rates  and  heading  are 
sent  to  the  navigation  filter  for  further  processing.  The  original  code  was  developed  by  J. 
Nicholson,  CAPT,  USN  and  modified  by  S.  Kragelund  for  this  research. 


/* - 

**  Modifications  to  Jack  Nicholson's  IMU.c  for  realtime  (8Hz)  operation 
**  This  program  downsamples  the  100Hz  IMU  data,  processing  one  message 
**  each  8Hz  interval.  Checksum  verification  has  been  added  to  ensure 
**  only  valid  data  gets  passed  to  the  Nav  process 
*  * 

**  SPK/DTD  07/14/06 

- */ 

#define  TRUE  1 
#def ine  FALSE  0 


#include  "IMU.h" 

//  SPK  010306:  Need  this  to  access  migration  library 
#include  <mig4nto.h> 

#include  <sys/neutrino . h> 

#include  <sys/netmgr . h> 


//  SPK  011106:  Added  for  Neutrino  timer  pulses 
#def ine  MP_PULSE_CODE  _PULSE_CODE_MINAVAIL 

#def ine  IMU_MSG_SIZE  44 

#define  PITCH  LIMIT  (60.0)  //  degrees 

#define  ROLL  LIMIT  (60.0)  //  degrees 


//  SPK  070506:  Define  values  for  IMU 
#def ine  IMU_ACCELJIEMP  OxFFOO  // 
#def ine  IMU_RLG_A_PLC  0x0080  // 
#def ine  IMU_RLG_B_PLC  0x0040  // 
#def ine  IMU_RLG_C_PLC  0x0020  // 
#def ine  IMU_FAILURE  0x0010  // 
#def ine  IMU_SW1_ERR_BITS  OxOOFO  // 
#def ine  IMU  COUNTER  0X000F  // 


status  word  #1  bits 
Bits  15-8  (LSB  =  1  deg  C) 

Bit  7  (a-axis  RLG  in  PLC  reset) 

Bit  6  (b-axis  RLG  in  PLC  reset) 

Bit  5  (c-axis  RLG  in  PLC  reset) 

Bit  4 
Bits  7-4 

Bits  3-0  (4-bit  counter) 


//  SPK  070506:  Define  values  for  IMU 
#def ine  IMU_PROC_FAILURE  0x8000  // 
#def ine  IMU_MEM_FAILURE  0x4000  // 
#def ine  IMU  OTHER  FAILURE  0x2000  // 


status  word  #2  bits 

Bit  15  (Processor  tests  failed) 

Bit  14  (Memory  tests  failed) 

Bit  13  (Other  tests  failed) 
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#define  IMU  ACCEL  FAILURE  0x1000  //  Bit  12  (Accelerometer  tests 
failed) 

#def ine  IMU_GYRO_FAILURE  0x0800  //  Bit  11  (Gyro  tests  failed) 

#def ine  IMU_RSV1_FAILURE  0x0400  //  Bit  10  (Reserved) 

#def ine  IMU_RSV2_FAILURE  0x0200  //  Bit  9  (Reserved) 

#def ine  IMU_RSV3_FAILURE  0x0100  //  Bit  8  (Reserved) 

#def ine  IMU_SW2_ERR_BITS  0xF800  //  Bits  15-11 

#def ine  IMU_SW_VER_NUMBER  OxOOFF  //  Bits  7-0  (Software  Version  #) 

//  SPK  070606:  Define  value  to  identify  a  checksum  error 
#def ine  IMU_CHKSUM_ERROR  0x0001 

FILE  * IMUalignfp; 

FILE  *NavLatfp; 

FILE  *Outfp;  //  Use  during  testing 

FILE  *DateStampInfp;  //  Use  during  testing 

double  pi=3 . 14159265; 

//  This  function  retrieves  the  last  IMU  message  in  the  buffer  at  time, 
//  verifies  its  checksum,  and  returns  an  integer  offset  for  the  first 
//  byte  of  the  message 
int  getLastMessage (buffer, numBytes) 
u  char*  buffer; 
int  numBytes; 

{ 

int  i; 

int  found  =  FALSE; 
unsigned  short  chkSum; 
unsigned  short  computedChkSum; 

if  (numBytes  <  IMU_MSG_SIZE) 
return  -1; 


for  (i  =  (numBytes  - 
{ 

if  ( ( * (buf fer+i )  : 
{ 

chkSum 

)  ; 

computedChkSum 


IMUJdSG_SIZE)  ;  ((i  >=  0)  &&  (!  found)  )  ;  i  — ) 

=  0xA5)  &&  ( * (buf fer+i+1 )  ==  0x02)) 

=  * (unsigned  short*) (buffer  +  i  +  IMU  MSG  SIZE 

=  * (unsigned  short*) (buffer  +  i  +  2)  + 

* (unsigned  short*) (buffer  +  i  +  4)  + 

* (unsigned  short*) (buffer  +  i  +  6)  + 

* (unsigned  short*) (buffer  +  i  +  8)  + 

* (unsigned  short*) (buffer  +  i  +  10)  + 

* (unsigned  short*) (buffer  +  i  +  12)  + 

* (unsigned  short*) (buffer  +  i  +  14)  + 

* (unsigned  short*) (buffer  +  i  +  16)  + 

* (unsigned  short*) (buffer  +  i  +  18)  + 

* (unsigned  short*) (buffer  +  i  +  20)  + 

* (unsigned  short*) (buffer  +  i  +  22)  + 

* (unsigned  short*) (buffer  +  i  +  24)  + 

* (unsigned  short*) (buffer  +  i  +  26)  + 

* (unsigned  short*) (buffer  +  i  +  28)  + 
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* (unsigned  short*) (buffer  +  i  +  30)  + 

* (unsigned  short*) (buffer  +  i  +  32)  + 

* (unsigned  short*) (buffer  +  i  +  34)  + 

* (unsigned  short*) (buffer  +  i  +  36)  + 

* (unsigned  short*) (buffer  +  i  +  38)  + 

* (unsigned  short*) (buffer  +  i  +  40); 

if  (chkSum  ==  computedChkSum) 

{ 

found  =  TRUE; 

i++; 

} 

else 

{ 

printf ( " IMU . c :  getLastMessage :  Bad  Checksum! \n" ) ; 

} 

}  //  if  (  ( *buf fer+1 . . . 

}  / /  for  ( i . . . 

//printf (" Index  %d\n",  i); 

if  (  i  <  0  ) 

{ 

printf (" IMU . c :  getLastMessage:  No  Valid  Record  Found! \n"); 

} 

return  i; 

}  //  getLastMessage 


main ( ) 

{ 

int  IMU  Shmid, IMUFlag  Shmid,  HMR  Shmid, read  status; 
int  i, k, i_r, i_m, Msg, StartByte; 
int  Port,  BytesRead; 

double  g  =  32.2; 

//  SPK  010306:  changed  from  char  to  unsigned  char  arrays  to  prevent 

//  compiler  warnings  due  to  comparisons  with  165 

/ /value 

//  (greater  than  maximum  value  for  signed  chars) 

u  char  ReadBuf [2000] ;  //  Buffer  for  data  coming  in  from  serial 

/ /port 

u  char  MsgBuf[44];  //  Message  buffer,  for  aligning  and 

//parsing  data 

char  FileCommand [256] ;  //  For  reading  latitude... 

char  FileString [256] ;  //  ...from  Nav.inp 

short  p,  q,  r,  ax,  ay,  az;  //  Fit  control  angular  rates  and  accel 

int  dAx,  dAy,  dAz,  dVx,  dVy,  dVz;  //  Inertial  delta  angles  / 

/ /velocities 

//  SPK  070506:  Added  to  verify  IMU  message  checksums 

unsigned  short  int  msgCkSum  =  0; 

unsigned  short  int  statusl  =  0; 

unsigned  short  int  status2  =  0; 

int  LastMsgldx  =  -1; 
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//  SPK  062606:  added  variables  for  first-order  accelerometer  filter 
double  Tau, Coef 1 , Coef2 , phi Thresh, thetaThresh; 
int  initFilter  =  FALSE; 

int  filterAcc  =  FALSE;  //  set  to  TRUE  for  first-order  filtering 


of  accelerometer  data 

double  p_out,  q_out,  r_out,  ax_out,  ay_out,  az_out;  // 

Variables  written. . . 

double  dAx_out, dAy_out, dAz_out, dPsi_out;  // 

...to  shared  memory. 

double  theta_out,  phi_out;  // 

2  Euler  angles 

double  dt;  // 

sampling  interval 

double  LatO,  LatD,  Lat;  // 

GPS  Origin  latitude 

double  s_phi, c_phi, s_theta, c_theta, s_psi, c_psi, s_Lat, c_Lat;  // 

Sines  and  cosines 

double  q_e,r_e;  // 

Earth  rates  in  q, r 

double  psi_out,  psi_dot,  dPsi;  // 

Heading 


//  SPK  062606:  Default  behavior  will  write  to  shared  memory  AND  file 
unless  FILE_FLAG  =  0 

int  FILE_FLAG=FALSE;  //  « —  Set  FILE^FLAG  to  zero  to  stop  writing 
100  Hz  data  to  a  file 

int  INIT_ERROR  =  0; 

int  I MU  Error=0;  //  Flag  to  abort  mission  upon 

IMU  failure 

unsigned  char  IMU  msgError  =1;  //  Flag  if  individual  message 

contains  an  error 

unsigned  short  int  IMU  msgErrBits  =  0;//  bits  within  status  words 
containing  error  bits 

unsigned  short  int  IMU  ErrorType  =0;  //  Bits  indicating  what  kind 
of  error  occurred 

int  IMU_Id=0; 

int  IMU_Kill=0 ; 

int  N  Samples=12;  //  Number  of  100Hz  samples  to  process  before 
writing  to  shared  memory 

int  nSamp  =0;  //  number  of  100Hz  samples  actually  used  to 

calculate  average 

double  Align  theta  =0.0;  //  Default  IMU  mechanical 

alignment . . . 

double  Align  phi  =0.0;  //  ...angles  (radians). 

double  C_Rate=pow (2 , -20 ) * 600 ;  //  Coefficients  by  which 

to  multiply  . . . 

double  C_Accel=pow (2 , -14 ) * 600 ;  //  ...summed  data  before 

writing  to  shared... 

double  C_DeltaAng=pow (2 , -33 ) ;  //  ...memory.  They  scale 

by  data  LSB, . . . 

double  C_DeltaV=pow (2 , -27 ) ;  //  ...and  average  flight 

control  data. 

double  Rotation=0 . 0 ;  //  Integrated  yaw  rate 

initialization 
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double  0mega=2*pi/ (24*60*60)  ; 
per  second 

double  DegRad  =  0.01745329; 
radians 


//  Earth  rate  in  radians 
//  Convert  degrees  to 


//  Open  I MU  shared  memory  ****  FOR  TESTING,  OPEN  IMUd  mmddyy  nn.d 
DATA  FILE 

//  SPK  071006:  allocate  space  for  null  character  in  s 
char  Filename [17] , s  [3]  ; 

char  ShellCom[]  =  "date  ' +IMUd_%m%d%y  . d'  >  IMUDateStamp" ; 

//  Real  Time  Loop  Stuff 
int  Hz, t  count; 
double  t; 

pid  t  LoopTimerProxy; 
timer  t  LoopTimerld; 
struct  itimerspec  LoopTimer; 
struct  sigevent  event; 

//  SPK  011106:  Added  for  Neutrino  timer  pulses 

int  timerChld; 

int  timerConld; 

int  timerRcvId; 

struct  _pulse  pulseMsg; 

Hz  =  8; 

dt  =  1.0  /  (double) Hz; 

//  SPK  010306:  Get  I/O  privity  for  this  thread  in  Neutrino 
//  (This  process  must  still  be  run  as  root) 

//  Note:  This  replaces  the  -T1  compiler  option  from  QNX4 
if  (  ThreadCtl  (  _NTO_TCTL_IO,  NULL  )  ==  -1  ) 

{ 

perror  ("I/O  Privilege  Request  failed!\n"); 
return ; 

} 

system (ShellCom) ; 
if (FILE_FLAG) 

{ 

DateStampInfp  =  fopen("IMUDateStamp","r"); 
f or ( k=0 ; k<12 ; ++k)  Filename[k]  =  getc (DateStampInfp) ; 
k=l  ; 

while  ( 1 ) 

{ 

if (k<10) 

sprintf (s, "0%d", k) ; 

else 

sprintf (s, "%d",  k)  ; 

Filename [12]  =  s[0]; 

Filename [13]  =  s[l]; 

Filename [14]  = 

Filename [15]  =  ' d ' ; 
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Filename [16]  =  NULL; 

//  SPK  062606:  Make  this  file  binary  for  size/speed  reasons 
if ( (Outfp=fopen (Filename, "r+b" ) ) ==NULL) 

{ 

Outfp  =  fopen (Filename, "wb" ) ; 

//  SPK  DEBUG 

if  (  Outfp  ==  NULL  ) 

{ 

printf ( "Cannot  open  output  file!\n"); 

exit (0) ; 

} 

break; 

} 

else 

{ 

fclose (Outfp) ; 

++k; 

if ( k==l 00 ) 

{ 

printf ( "Cannot  Create  File  Number  100\n"); 

exit (0) ; 

} 

} 

}  //--  end  while(l)  -- 
}  // —  end  if (FILE_FLAG)  — 

/ /  Open  IMU  shared  memory 

if ( (IMU^Shmid  =  OpenlMUShm ( ) )  ==  -1) 

{ 

printf ( "Cannot  Attach  IMU  Shared  Memory\n") ; 

INIT^ERROR  =  1; 

} 

if ( (IMUFlag_Shmid  =  OpenlMUFlagShm ( ) )  ==  -1) 

{ 

printf ( "Cannot  Create  IMU  Flag  Shared  Memory\n") ; 

INIT_ERROR  =  1; 

} 

if (INIT^ERROR)  exit(0); 

ResetIMUFlagShm ( IMUFlag_Shmid) ; 

//  Open  HMR  shared  memory  to  get  psi  value  to  initialize  to 
if ( (HMR^Shmid  =  OpenHMRShm ( ) )  ==  -1) 

{ 

printf ( "Cannot  Attach  HMR  Shared  Memory\n") ; 

INIT^ERROR  =  1; 

} 

sleep (2);  //  To  ensure  HMR  has  a  value  to  initialize  to 

ReadHMRShm (HMR_Shmid, &psi_out) ; 

CloseHMRShm (HMR_Shmid)  ; 

printf ("Initial  psi=%f  degrees. \n",  psi_out  ); 
psi_out=psi_out*DegRad; 
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//--  Read  IMU  angle  offsets  from  IMU.inp  -- 
IMUalignfp  =  fopen ("IMU. inp", "r") ; 
if (IMUalignfp)  fscanf (IMUalignfp, "%lf 
lf\n",&Align  theta, SAlign  phi); 
f close ( IMUalignfp) ; 

//--  Read  latitude  from  Nav.inp  -- 

OpenlnputFile ( )  ; 

while  ( 1 ) 

{ 

read_status  =  ReadFromlnputFile ( SFileString  [  0 ] ) ; 
if ( read_status  >  0) 

{ 

sscanf (FileString, "%s" , SFileCommand [ 0 ] ) ; 
if ( ! strcmp (FileCommand, "END" ) )  break; 

} 

else  if ( ! strcmp (FileCommand, "SET_GPS_ORIGIN" ) ) 

{ 

sscanf (FileString, "%s  %lf", SFileCommand [ 0 ] , &LatO) ; 

//  Convert  to  dd.dddd 

LatD  =  (double)  (  (int)  (Lat0/100 . 0) ) ; 

Lat  =  LatD  +  (LatO  -  LatD*100 . 0) /60 . 0; 

} 

} 

CloselnputFile ( ) ; 

s_Lat=sin (Lat*pi/180)  ; 
c_Lat=cos (Lat*pi/180)  ; 

//--  Open  and  flush  the  RS-422  port  -- 
Port  =  open ( " /dev/ser4 " ,  O^RDONLY  |  0_N0CTTY) ; 

BytesRead  =  2000; 
while (BytesRead  ==  2000) 

{ 

BytesRead  =  dev_read (Port, ReadBuf,  2000, 0, 0, 0, 0, 0)  ; 

} 

//  Initial  load  of  message  buffer 

BytesRead  =  dev_read ( Port ,  ReadBuf ,  2000 , 44 , 0 , 0 , 0 , 0 )  ; 
for (i=0; i<44; i++) MsgBuf [i] =ReadBuf [i] ; 

//  SPK  011106:  Create  channel  and  connection  for  timer  pulse 
timerChld  =  ChannelCreate ( 0 ) ; 

timerConld  =  ConnectAttach (ND_LOCAL_NODE,  0,  timerChld, 

NTO_S I DE_CHANNEL ,  0); 

/*  Attach  to  the  Timer  */ 

//  SPK  011106:  Initialize  pulse  event  using  Neutrino  macro 
SIGEV_PULSE_INIT (  Sevent,  timerConld,  getprio(O),  MP_PULSE_CODE,  0 


//  SPK  010306:  Neutrino  version  returns  timer  ID  in  third  parameter 
//  old:  LoopTimerld  =  timer  create (CLOCK  REALTIME, Sevent) ; 
timer_create  (CLOCK_REALTIME,  Sevent,  SLoopTimerld) ; 
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if (LoopTimerld  ==  -1) 

{ 

printf (  "Unable  to  Attach  Timer."  ); 
return; 

} 

/* 

*  1  nano-seconds  before  initial  firing, 

*  1.0/Hz  second  repetitive  timer  afterwards. 

*/ 

LoopTimer.it  value. tv  sec  =  OL; 

LoopTimer.it  value. tv  nsec  =  1L; 

LoopTimer.it  interval. tv  sec  =  OL; 

/*  Convert  Hz  into  NanoSecond  Period  */ 

LoopTimer.it  interval. tv  nsec  =  (int)  (1 . 0/ ( (float) 

Hz ) *pow (10.0,9.0) ) ; 

timer_settime (LoopTimerld, 0, SLoopTimer , NULL) ; 

/*********************************************************************/ 
/*--  Main  loop  -  once  per  shared  memory  write  cycle 
(N  samples*10msec)  --*/ 

/*********************************************************************/ 
i  m=44; 
i_r=0 ; 
while ( 1 ) 

{ 

/*  Wait  for  the  Proxy  */ 

//  SPK  011106:  No  longer  using  old  code  (or  mig4nto  version) 

/ / old:  Receive (LoopTimerProxy,  0,0)  ; 

timerRcvId  =  MsgReceive (timerChld,  SpulseMsg,  sizeof (pulseMsg) , 

NULL)  ; 


//  SPK  011106:  Can  check  if  timerRcvId  ==  0  to  verify  a  pulse  was 
received  and 

//  can  check  if  pulseMsg . code  ==  MP  PULSE  CODE  to  verify  it's 
from  our  timer 

/****************************************************/ 

/*--  Read  loop  -  once  per  IMU  data  message  period  --*/ 
/****************************************************/ 

IMU_Error  =  0; 

BytesRead  =  dev_read ( Port ,  ReadBuf,  2000 , 44 , 0 , 0 , 0 , 0 )  ; 

//printf ( "bytes  read  =  %d\n",  BytesRead); 

LastMsgldx  =  getLastMessage (ReadBuf , BytesRead) ; 

if  (LastMsgldx  <  0  ) 

{ 

IMU  Error  =  1; 

} 

else 

{ 
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//  parse  the  message 

p  =* (short*)  (ReadBuf +LastMsgIdx+2 )  ;  q 
=* (short*) (ReadBuf +LastMsgIdx+4 ) ;  r  =* (short*) (ReadBuf+LastMsgIdx+6) ; 

ax  =* (short*) (ReadBuf +LastMsgIdx+8 ) ;  ay 
=* (short*) (ReadBuf +LastMsgIdx+l 0 ) ;  az 
=* (short*) (ReadBuf +LastMsgIdx+12 ) ; 

statusl  =  * (unsigned  short*)  (ReadBuf+LastMsgIdx+14 )  ; 
status2=* (unsigned  short*) (ReadBuf+LastMsgIdx+16) ; 

dAx=* (int*)  (ReadBuf +LastMsgIdx+l 8 )  ; 
dAy=* (int*) (ReadBuf +LastMsgIdx+22 ) ; 
dAz=* (int*) (ReadBuf+LastMsgIdx+26) ; 

dVx=* (int*) (ReadBuf +LastMsgIdx+30 ) ; 
dVy=* (int*) (ReadBuf +LastMsgIdx+34 ) ; 
dVz=* (int*) (ReadBuf+LastMsgIdx+38) ; 

msgCkSum=* (unsigned  short*)  (ReadBuf +LastMsgIdx+42 )  ; 

p_out  =  (( (double) p)  *  C_Rate) ; 
q_out  =  (( (double) q)  *  C_Rate) ; 
r_out  =  (( (double) r)  *  C_Rate) ; 
ax_out  =  (( (double) ax)  *  C_Accel) ; 
ay_out  =  (( (double) ay)  *  C_Accel) ; 
az_out  =  (( (double) az)  *  C_Accel) ; 

//  Note:  IMU  outputs  negative  acceleration  values 
g  =  ( (ax_out  ==  0.0)  &&  (ay_out  ==  0.0)  &&  (az_out  ==  0.0))  ? 

32.2  : 

pow ( (pow (ax_out, 2 ) +pow (ay_out, 2 ) +pow (az_out, 2 ) ) , 0 . 5) ; 

//  Bound  input  to  asin  function  to  prevent  NANs 
if  (  fabs (ax_out/g)  <1.0  ) 

{ 

theta_out  =  asin (ax_out/g) ; 

} 

else 

{ 

theta_out  =  (  (fabs (ax_out) )  /  (ax_out)  )  * 
(PITCH_LIMIT*DegRad) ; 

} 

//  Bound  input  to  asin  function  to  prevent  NANs 
if  (  fabs (ay_out/ (g*cos (theta_out) ) )  <1.0  ) 

{ 

phi_out  =  -asin (ay_out/ (g*cos (theta_out) ) ) ; 

} 

else 

{ 

phi_out  =  -(  (fabs (ay_out) )  /  (ay_out)  )  * 

(ROLL_LIMIT*DegRad)  ; 

} 

//  Remove  earth  rate,  convert  angular  rates  into  heading  rate 

(psi  dot) 

s_phi  =  sin (phi_out) ;  c_phi  =  cos (phi_out) ; 

s_theta=  sin (theta_out) ;  c_theta=  cos (theta_out) ; 
s_psi  =  sin (psi_out) ;  c_psi  =  cos (psi_out) ; 

q_e= (-c_phi*s_psi*c_Lat-s_phi*c_theta*s_Lat) *0mega; 
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r  e=((s  phi*s  psi+c  phi*s  theta*c  psi)*c  Lat- 
c_phi*c_theta*s_Lat) *Omega; 

q_out=q_out-q_e; 

r_out=r_out-r_e; 

psi_dot  =  s_phi*q_out/c_theta  +  c_phi*r_out/c_theta; 
dPsi  =  psi_dot*dt; 
psi_out  =  psi_out+dPsi ; 

}  //  if  (LastMsgldx . . .  else... 

ReadlMUFlagShm ( IMUFlag_Shmid, &IMU_Kill) ; 
if(IMU  Kill)  break; 

// -  SHARED  MEMORY  OUTPUT  - 

WritelMUShm ( IMU_Shmid, IMU_Id, phi_out, theta_out, psi_out, p_out, q_out, r_ou 
t, ax_out, 

ay_out, az_out, 60.0,60.0, IMU_Error) ; 

IMU_Id++; 

if (IMU_Id>65534)  IMU_Id  =  0; 

}  / /  while ( 1 ) 

ConnectDetach (  timerConld  ); 

/*  Get  Rid  of  the  Timer  */ 
timer_delete (LoopTimerld)  ; 

if (FILE_FLAG) 

fclose (Outfp) ; 

close (Port) ; 

CloselMUShm ( IMU_Shmid) ; 

CloselMUFlagShm ( IMUFlag_Shmid) ; 

/ / CloseHMRShm (HMR  Shmid)  ; 
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APPENDIX  E:  VEHICLE  NAVIGATION  FILTER  CODE 


The  following  provides  the  C-code  for  the  navigation  filter.  This  code  has  been 
developed  over  many  years  of  experience  and  was  originally  written  by  D.  Marco  and  A. 
Healey  for  use  in  the  ARIES  vehicle. 


/* - 

**  SPK  010306:  Modified  for  QNX  Neutrino  6.3 

-  comment  out  includes  for  obsolete  files  in  Nav.h 

-  include  migration  library  header  file  in  Nav.h 

-  start  the  migration  process  manager  (mig4nto  init) 

-  updated  timer_create ( )  to  Neutrino  version 

**  SPK  010906:  Replaced  Creceive ()  with  qnx  proxy  detach () 

**  SPK  011106:  Implemented  Neutrino  timer  pulses  instead  of 

migration  library  cover  functions 

-  no  longer  need  migration  process  manager 

*  * _ 

- */ 

#include  "Nav.h" 

//  SPK  011106:  Included  for  Neutrino  timer  pulses 
#include  <sys/neutrino . h> 

#include  <sys/netmgr . h> 

#define  TRUE  1 
#def ine  FALSE  0 

//  SPK  011106:  Added  for  Neutrino  timer  pulses 
#def ine  Nav_PULSE_CODE  _PULSE_CODE_MINAVAIL 

FILE  *Outfp; 

FILE  *NavErrorfp; 

FILE  * Inputfp; 

FILE  *AbortLogfp; 

char  FileString [256] ;  /*  This  is  the  Entire  Line  of  the  File  */ 
char  FileCommand [256] ;  /*  This  is  the  File  Command  i.e.  Leading 
Text  */ 

double  pi  =  3.14159265358979; 

double  RadDeg  =  57.2957795; 
double  DegRad  =  0.01745329; 

/*  Wait  until  RDI  AltEst  is  not  equal  to  0  -  added  by  ajh  12/1/04 

*/ 

/*  Altitude  Filter  Stuff  */ 
int  KALMAN_INIT  =  TRUE; 

/*  added  for  RDI  wait  12/01/04*/ 
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int  WAIT  =  FALSE; 
int  count  =  0; 


double  Alt_est  =  0.0; 
double  Alt_dot  =  0.0; 
double  Alt_ddot  =  0.0; 
double  RDI  AltRawPrev  =  0.0; 


FILE  *TestFilefp; 

main ( ) 

{ 

int  read_status; 
double  MaxDepth; 
double  MinBatteryVoltage; 
double  MaxLeakVoltage; 

int  i, j , k, kk, j j , ip; 
int  INIT_ERROR  =  FALSE; 

int  ABORT_FLAG  =  FALSE; 

int  KALMAN_ABORT_FLAG  =  FALSE; 
int  Nav_Kill  =  FALSE; 

/*  Real  Time  Loop  Stuff  */ 

int  Hz,t  count; 

double  t,dt; 

pid  t  LoopTimerProxy; 

timer  t  LoopTimerld; 

struct  itimerspec  LoopTimer; 

struct  sigevent  event; 

//  SPK  011106:  Added  for  Neutrino  timer  pulses 

int  timerChld; 

int  timerConld; 

int  timerRcvId; 

struct  _pulse  pulseMsg; 

/*  Absolute  Date  &  Time  Stuff  */ 

struct  timeb  timebuf; 

time_t  time_of_day; 

char  buf [26] ; 

struct  tm  tmbuf; 

char  TacticalMessage [16]  ; 

/*  date  "+%m%d%y%H%M%S"  */ 

int  Nav  Shmid; 
int  NavFlag  Shmid; 
int  NetFlag  Shmid; 
int  RDI  Shmid; 
int  MP  Shmid; 
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int  GPS  Shmid; 

int  Analog  Shmid; 

int  Bob  Shmid; 

int  HMR  Shmid; 

double 

LatD, LongD; 

double 

KG  psi; 

int 

Nav  Id 

=  0;  /*  Navigation  ID  Process  (1-65535) 

*/ 

int 

Month; 

int 

Day; 

int 

Year; 

int 

Hour; 

/*  24  Hour  GMT 

*/ 

int 

Minute; 

double 

Second; 

/*  Seconds  and  Fraction  of  Seconds 

*/ 

double 

DepthRaw 

=0.0;  /*  Raw  Depth  from  Depth  Cell 

*/ 

/*  (No  Filtering)  (m) 

*/ 

double 

DepthEst 

=  0.0;  /*  Estimated  Depth  from  Depth  Filter (m)*/ 

double 

DepthDot 

=  0.0;  /*  Estimated  Depth  Rate  from  Depth 

*/ 

/*  Filter  (m/sec) 

*/ 

int 

Nav  Error 

=  FALSE; 

int 

RDI  Id; 

/*  RDI  ID  Process  (1-65535) 

*/ 

double 

RDI  Ug; 

/*  RDI  U  Ground  (m/sec) 

*/ 

double 

RDI  Vg; 

/*  RDI  V  Ground  (m/sec) 

*/ 

double 

RDI  Wg; 

/*  RDI  W  Ground  (m/sec) 

*/ 

double 

RDI  Uf; 

/*  RDI  U  Fluid  (m/sec) 

*/ 

double 

RDI  Vf; 

/*  RDI  V  Fluid  (m/sec) 

*/ 

double 

RDI  Wf; 

/*  RDI  W  Fluid  (m/sec) 

*/ 

double 

RDI  AltRaw; 

/*  Raw  RDI  Altitude  (m) 

*/ 

double 

RDI^AltEst; 

/*  Estimated  Altitude  from  Alt  Filter  (m) 

*/ 

double 

RDI  AltDot; 

/*  Estimated  Altitude  Rate  from  Filter  (m/sec 

)  */ 

double 

RDI  Heading; 

/*  RDI  Heading  (Degrees) 

*/ 

double 

psi; 

/*  RDI  Heading  (Radians) 

*/ 

int 

RDI  Error; 

int 

HMR  Id; 

/*  HMR3000  Compass  Process  ID  (1-65535) 

*/ 

double 

HMR  HeadingRaw;  /*  HMR3000  Uncompensated  Compass  Heading 

(Degrees)  */ 

double 

HMR  Heading; 

/*  HMR3000  Compass  Heading  Angle  (Degrees) 

*/ 

double 

HMR  Pitch; 

/*  HMR3000  Compass  Pitch  Angle  (Degrees) 

*/ 

double 

HMR  Roll; 

/*  HMR3000  Compass  Roll  Angle  (Degrees) 

*/ 

int 

HMR  Error; 

/*  Kalman  Filter  Stuff  */ 
double  RDI  AltRawComp; 
int  AltZeroCount  =  0; 
int  SKIP_KALMAN  =  FALSE; 
double  PrevRDI  AltRawComp  =  0.0; 

//  SPK  060206:  Update  variable  definitions  to  reflect  IMU  process 
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//  Note:  still  need  to  rename  all  process  and  shared  memory 
variables 


int 

MP  Id; 

II  IMU  Process  ID  (1-65535) 

double 

MP  phi; 

//  IMU  Roll  Angle  (Rad) 

double 

MP  theta; 

//  IMU  Pitch  Angle  (Rad) 

double 

MP  psi; 

//  IMU  Heading  Angle  (Rad) 

double 

KG_q; 

//  IMU  Roll  Rate  (Rad/Sec) 

double 

MP  q; 

//  IMU  Pitch  Rate  (Rad/Sec) 

double 

MP  r; 

//  IMU  Yaw  Rate  (Rad/Sec) 

double 

MP  XAccel; 

//  IMU  X-axis  Acceleration  (m/secA2) 

double 

MP  YAccel; 

//  IMU  Y-axis  Acceleration  (m/secA2) 

double 

KG  r; 

//  IMU  Z-axis  Acceleration  (m/secA2) 

double 

BatteryVoltageRaw;  / /  Raw  Battery  Voltage  Level 

(Volts ) 

double 

Bat ter yVo It age 

r 

//  Filtered  Battery  Voltage  Level  (Volts 

) 

int 

MP  Error; 

//  IMU  Error 

int 

GPS  Id; 

/* 

GPS  Process  ID  (1-65535) 

*/ 

int 

GPS  Signal; 

/* 

Status  of  Signal  1  =  Present,  0 

=  Not 

Present 

*/ 

double 

LatO ; 

/* 

GPS  Latitude  Origin  in  ddmm.mmmmmm 

*/ 

double 

LongO ; 

/* 

GPS  Longitude  Origin  in  dddmm.mmmmmm 

*/ 

double 

LatDegO ; 

/* 

GPS  Latitude  Origin  in  dd.ddddd 

*/ 

double 

LongDegO ; 

/* 

GPS  Longitude  Origin  in  ddd.ddddd 

*/ 

double 

GPS  X; 

/* 

Distance  in  meters  North/South 

from  GPS 

Latitude  ' 

*/ 

/* 

Origin  (LatO) 

*/ 

double 

GPS  Y; 

/* 

Distance  in  meters  East/West  from  GPS 

Longitude 

*/ 

/* 

Origin  (LongO) 

*/ 

int 

Dif  f  ; 

/* 

Raw  or  Diff. 

*/ 

int 

NSv; 

/* 

Number  of  SVs 

*/ 

double 

ToC; 

/* 

Time  of  Computation 

*/ 

double 

Lat; 

/* 

Latitude  in  ddmm.mmmmmm 

*/ 

double 

LatDeg; 

/* 

Latitude  in  dd.ddddd 

*/ 

double 

Long; 

/* 

Longitude  in  dddmm.mmmmmm 

*/ 

double 

LongDeg; 

/* 

Longitude  in  ddd.ddddd 

*/ 

double 

SbA; 

/* 

Sensor-Based  Altitude 

*/ 

double 

TtTc; 

/* 

True  Track/True  Course 

*/ 

double 

SoG; 

/* 

Speed  over  Ground 

*/ 

double 

Vv; 

/* 

Vertical  Velocity 

*/ 

double 

Pdop; 

/* 

Position  dilution  of  position 

*/ 

double 

Hdop; 

/* 

Horiz.  dilution  of  position 

*/ 

double 

Vdop; 

/* 

Vertical  dilution  of  position 

*/ 

double 

Tdop; 

/* 

Time  dilution  of  position 

*/ 

int 

GPS  Error; 

int 

Analog  Id; 

/*  Analog  Process  ID  (1-65535 

) 

*/ 

double 

BowLeakVoltage 

r 

/*  Bow  Section  Leak  Detector 

Voltage 

*/ 

/*  Level  (Volts) 

*/ 

double 

MidLeakVoltage 

r 

/*  Mid  Section  Leak  Detector 

Voltage 

*/ 

/*  Level  (Volts) 

*/ 

double 

SternLeakVoltage; 

/*  Stern  Section  Leak  Detector  Voltage 

*/ 

/*  Level  (Volts) 

*/ 

double 

v  Is; 

/*  Left  Screw  Speed  (Rot/Sec) 

*/ 

double 

v  rs ; 

/*  Right  Screw  Speed  (Rot/Sec 

) 

*/ 
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double  v_bvt; 
double  v_svt; 
double  v_blt; 
double  v_slt; 
int  Analog  Error; 


/*  Bow  Vertical  Thruster  Speed  (Rot/Sec)*/ 
/*  Stern  Vertical  Thruster  Speed  (Rot/Sec)  */ 
/*  Bow  Lateral  Thruster  Speed  (Rot/Sec)  */ 
/*  Stern  Lateral  Thruster  Speed  (Rot/Sec)  */ 


/*  Filter  Vars  */ 


/*  These  are  States  (Outputs,  Estimates  from  the  Filter)  */ 

double  NavFil  X;  /*  Global  X  Position  (meters)  Relative  to  */ 

/*  a  GPS  Origin  (LatDegO, LongDegO)  */ 

double  NavFil  Y;  /*  Global  Y  Position  (meters)  Relative  to  */ 

/*  a  GPS  Origin  (LatDegO, LongDegO)  */ 

double  NavFil  psi;  /*  Yaw  Angle  (Radians)  */ 

double  NavFil_r;  /*  Yaw  Rate  (Rad/sec)  */ 

double  NavFil  Ug;  /*  Longitudinal  Ground  Speed  (m/sec)  */ 

double  NavFil  Vg;  /*  Lateral  Ground  Speed  (m/sec)  */ 

double  NavFil  Bias  r;  /*  Yaw  Rate  Bias  (Rad/sec)  */ 

double  NavFil  Bias  psi;  /*  Yaw  angle  Bias  (Radians)  */ 


double  y [ 1 ] [ 8 ] , ym [ 8 ] ; 

double  18  [9]  [9] ; 
double  x [ 9] ; 
double  x_barl[9]; 

double  res  [ 8 ]  [2 ] , ym_prev [ 8 ] , yhat [ 8 ] ; 
double  psiO; 

double  spsi,cpsi; 
double  Adt[9] [9]; 
double  C [ 8 ]  [9] ; 
double 

P [ 9] [ 9] , M [ 9] [ 9] , LC [ 9] [9], ImLC [9] [ 9] , P_diag_out [ 9] , P_l_8_out, P_2_8_out; 
double  phi [9]  [9] ,phi_t [9]  [9] , Pphi_t [9]  [9] , phiPphiJ;  [ 9]  [9] ; 
double  C_t [9] [8] ,MC_t [9] [8] ,CMC_t [8] [8] ; 
double  CMCR [ 8 ]  [8] , CMCRI [8]  [8]  ,  L[9]  [8], Lres [9]  [2] ; 
double  Q [ 9] [9] ; 
double  R [ 8 ] [ 8 ] ; 
double  q [ 9] , nu [ 8] , p [ 9] ; 


/*  Process  Vars  */ 


int 

LastRDI  Id 

=  0; 

int 

RDI  IdDupCnt 

=  0; 

int 

MaxRDI  IdDupCnt 

=  10; 

int 

LastGPS_Id 

=  0; 

int 

GPS  IdDupCnt 

=  0; 

int 

MaxGPS  IdDupCnt 

=  12; 

int 

LastMP  Id 

=  0; 

int 

MP  IdDupCnt 

=  0; 

int 

// 

MaxMP  IdDupCnt 

SPK  071806 

=  10; 
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int  LastMP  Error  =  0; 

int  MP  ErrorCnt  =  0; 

int  MaxMP  ErrorCnt  =  10; 

int  LastAnalog  Id  =0; 

int  Analog  IdDupCnt  =  0; 

int  MaxAnalog  IdDupCnt  =  10; 

int  LastHMR_Id  =  0; 

int  HMR  IdDupCnt  =  0; 

int  MaxHMR  IdDupCnt  =  10; 


double  Coefl; 

Coefl  =  3443.9* (1852.47) * (pi/180.0)  ; 
/*Coef 1  =  111318.8938906694;*/ 


for(i=l;i<=8;++i) 

{ 


for ( j=l; 

j< 

=  8; 

++j) 

{ 

Adt  [  i 

]  [ 

j] 

=  0. 

0; 

M[i]  [ 

j] 

=  0. 

0; 

P[i]  [ 

j] 

=  0. 

0; 

Q[i]  [ 

j] 

=  0. 

0; 

18  [i] 

[j 

] 

=  0. 

0; 

} 

} 

f  o 

r ( i=l ; i<= 

:8  ; 

++i 

) 

{ 

18 [i] [i] 

= 

1 . 

0; 

x  barl  [  i 

] 

=  0 

.0; 

} 

f  o 

r ( i=l ; i<= 

-1; 

++i 

) 

{ 

for ( j=l; 

j< 

=7; 

++j) 

{ 

R[i]  [ 

j] 

= 

0.0 

r 

} 

} 

f  o 

r ( i=l ; i<= 

-1; 

++i 

) 

{ 

for ( j=l; 

j< 

=  8; 

++j) 

{ 

C  [  i  ]  [ 

j] 

= 

0.0 

r 

} 


/*  Measurement  Vector: 

y  =  [Ug, Vg, psi , r , LatDeg, LongDeg] ;  */ 
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/*  New  Measurement  Vector: 

y  =  [Ug, Vg, psi , r , LatDeg, LongDeg, KG_psi ] ; 
KG  psi  =  Integrted  KG  r; 


*/ 


Hz  =  8; 

t  =  0.0; 

t_count  =  0; 

dt  =  1 . 0/ ( (double)  Hz); 

NavErrorfp  =  fopen ( "NavError . d" , "w" ) ; 

AbortLogfp  =  fopen ( "Abort . Log" , "w" ) ; 

OpenNavDataFile ( )  ; 

OpenlnputFile ( ) ; 

if ( (Nav_Shmid  =  OpenNavShm ( )  )  ==  -1) 

{ 

printf ( "Cannot  Attach  Nav  Shared  Meraory\n") ; 
INIT^ERROR  =  TRUE; 

} 

if ( (NavFlag_Shmid  =  OpenNavFlagShm ( ) )  ==  -1) 

{ 

printf ( "Cannot  Create  NavFlag  Shared  Memory\n") 
INIT^ERROR  =  TRUE; 

} 

if ( (NetFlag_Shmid  =  OpenNetFlagShm ( ) )  ==  -1) 

{ 

printf ( "Cannot  Create  NetFlag  Shared  Memory\n") 
INIT^ERROR  =  TRUE; 

} 

if ( (RDI_Shmid  =  OpenRDIShm ( ) )  ==  -1) 

{ 

printf ( "Cannot  Attach  RDI  Shared  Meraory\n") ; 
INIT^ERROR  =  TRUE; 

} 

if ( (MP_Shmid  =  OpenMotPakShm ( ) )  ==  -1) 

{ 

printf ( "Cannot  Attach  MotPak  Shared  Memory\n"); 
INIT^ERROR  =  TRUE; 

} 

if ( (GPS_Shmid  =  OpenGPSShm ( ) )  ==  -1) 

{ 

printf ( "Cannot  Attach  GPS  Shared  Meraory\n") ; 
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INIT_ERROR  =  TRUE; 

} 

if ( (Analog_Shmid  =  OpenAnalogShm ( ) )  ==  -1) 

{ 

printf ( "Cannot  Attach  Analog  Shared  Meraory\n") ; 
INIT_ERROR  =  TRUE; 

} 

if ( (Bob_Shmid  =  OpenBobShm ( ) )  ==  -1) 

{ 

printf ( "Cannot  Attach  Bob  Shared  Meraory\n") ; 
INIT  ERROR  =  TRUE; 


if ( (HMR^Shmid  =  OpenHMRShm ( ) )  ==  -1) 

{ 

printf ( "Cannot  Attach  HMR  Shared  Meraory\n") ; 

INIT^ERROR  =  TRUE; 

} 

if (INIT^ERROR)  exit(O); 

ResetNavFlagShm (NavFlagShmid) ; 

WriteTacticalMessage (NetFlag_Shmid,  "GO")  ; 

/*  Get  Initialization  Values  */ 
while (TRUE) 

{ 

read_status  =  ReadFromlnputFile ( SFileString [ 0 ] ) ; 

if (read_status  >  0) 

{ 

sscanf (FileString, "%s", SFileCommand [ 0 ] ) ; 

/*  Break  if  End  */ 

if ( ! strcmp (FileCommand,  "END" ) ) 

{ 

break; 

} 

else  if ( ! strcmp (FileCommand, "SET_MAX_DEPTH" ) ) 

{ 

sscanf ( FileString, "%s  %lf", & FileCommand [ 0 ] , SMaxDepth) 

} 

else  if ( ! strcmp (FileCommand, "SET_GPS_ORIGIN" ) ) 

{ 

sscanf (FileString, "%s  %lf  %lf " , SFileCommand [ 0 ] , 

&LatO , SLongO ) ; 

/*  Convert  to  dd.dddd  */ 

LatD  =  (double)  ( (int)  (Lat0/100 . 0) ) ; 

LongD  =  (double)  ((int)  (Long0/100 . 0) ) ; 
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LatDegO  =  LatD  +  (LatO  -  LatD*100 . 0) /60 . 0; 

LongDegO  =  LongD  +  (LongO  -  LongD* 1 00 . 0 ) /60 . 0 ; 

} 

else  if ( ! strcmp (FileCommand, "SET_MIN_BATTERY_VOLTAGE" ) ) 

{ 

sscanf (FileString, "%s 
%lf", SFileCommand [ 0 ] , SMinBatteryVoltage) ; 

} 

else  if ( ! strcmp (FileCommand, " SET_MAX_LEAK_VOLTAGE " ) ) 

{ 

sscanf (FileString, "%s 
%lf", SFileCommand [ 0 ] , SMaxLeakVoltage)  ; 

} 

else 

{ 

printf ( "FileCommand  Not  Recognized\n" ) ; 

} 

} 

} 

CloselnputFile ( )  ; 

/*  Read  Initial  Measurement  Vector  and  Wait  until  RDI  AltRaw  is  not 
equal  to  0  -  ajh  12/1/04  */ 
while (WAIT) 

{ 


ReadRDIShm (RDI__Shmid, &RDI_Id, &RDI_Ug, &RDI_Vg, &RDI_Wg, 
&RDI_Uf , &RDI_Vf , &RDI_Wf , 

&RDI  AltRaw, &RDI  Heading, 

&RDI  Error) ; 

sleep (1) ; 

if  (RDI^AltRaw  >  0.0) 

{ 

count  =  count+1; 

} 

if  (count  ==  3) 

{ 

WAIT=FALSE; 

} 


LastRDI_Id  =  RDI_Id; 

/*  Read  Initial  Measurement  Vector  */ 

ReadHMRShm (HMR_Shmid, &HMR_Id, &HMR_HeadingRaw, &HMR_Heading,  &HMR_Pitch, 

&HMR_Roll , 

&HMR  Error) ; 


LastHMR_Id  =  HMR_Id; 

ReadMotPakShm (MP_Shmid, &MP_Id, &MP_phi, &MP_theta, &MP_psi, 

&KG  q, &MP  q, &MP  r. 
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&MP  XAccel,&MP  YAccel,&KG  r, SBatteryVoltageRaw, 

SBatteryVoltage, 

&MP  Error) ; 


//  SPK  09/01/05 
psi  =  MP  psi; 

LastMP_Id  =  MP_Id; 

//  SPK  071806 

LastMP  Error  =  MP  Error; 


ReadGPSShm (GPS_Shmid, &GPS_Id, &GPS_Signal, &Diff, &NSv, &ToC,  &Lat,  SLatDeg, 

&Long, SLongDeg, &SbA, &TtTc, &SoG, &Vv, 

&Pdop, &Hdop, &Vdop, &Tdop, &GPS_Error) ; 


LastGPS_Id  =  GPS_Id; 

ReadAnalogShm (Analog_Shmid, &Analog_Id, SDepthRaw, SDepthEst, SDepthDot, 
SBowLeakVoltage, SMidLeakVoltage, &SternLeakVoltage, 

&v  ls,&v  rs,&v  bvt, &v  svt,  &v  bit,  &v  sit, SAnalog  Error); 

LastAnalog  Id  =  Analog  Id; 

if ( ! GPS  Signal ) 

{ 

/*  We  Don't  have  a  GPS  Signal,  Zero  GPS_X  and  GPS_Y  */ 

GPS_X  =  0.0; 

GPS_Y  =  0.0; 

} 

else 

{ 

GPS_X  =  Coefl* (LatDeg  -  LatDegO) ; 

GPS_Y  =  Coefl* (LongDeg  -  LongDegO) *cos (DegRad*LatDeg)  ; 

} 

/*  Assign  Initial  Measurement  Vector  */ 
ym [ 1 ]  =  RDIJJg; 

//  SPK  09/01/05 
/ / ym [ 2 ]  =  RDI_Vg-l . 0*KG_r; 
ym [2 ]  =  RDI_Vg-l . 0*MP_r; 

//  SPK  021606:  Add  delay  to  ensure  we  read  the  latest  value  from  IMU 

sleep  (1)  ; 

ym[3]  =  psi; 

//  SPK  09/01/05 

//ym[4]  =  KG  r;  /*  Was  MP  r  */ 
ym [ 4 ]  =  MP  r ; 
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ym [ 5 ]  =  GPS_X; 
ym [ 6]  =  GPS_Y; 


//  SPK  09/01/05 

//ym[7]  =  KG  psi;  /*  This  is  a  Virtual  Heading  Ref.  */ 
ym[7]  =  psi; 

/*  State  Vector: 

x  =  [NavFil  X  NavFil  Y  NavFil  psi  NavFil  r  NavFil  Ug  NavFil  Vg 
NavFil  Bias  r  NavFil  Bias  psi];  */ 


X[l]  = 

ym [ 5 ] ;  / * 

Initial 

GPS 

X  */ 

x  [2  ]  = 

ym [ 6 ] ;  /* 

Initial 

GPS~ 

_Y  *  / 

x[3]  = 

ym [ 3 ] ; 

x  [  4  ]  = 

ym [ 4 ] ; 

x  [  5  ]  = 

ym [ 1 ] ; 

x  [  6]  = 

ym [ 2 ] ; 

x  [  7  ]  = 

0.0;  /* 

Bias  =  0 

at 

Start 

x  [  8  ]  = 

0.0;  /* 

Bias  =  0 

at 

Start 

/*  Init  the  State 

Estimate 

Vector  * 

NavFil 

X 

x  [  1  ]  ; 

NavFil 

"y 

x  [  2  ]  ; 

NavFil 

psi  = 

x  [  3  ]  ; 

NavFil 

r  = 

x  [  4  ]  ; 

NavFil 

Ug 

x  [  5  ]  ; 

NavFil 

Vg 

x  [  6  ]  ; 

NavFil 

Bias  r  = 

x  [  7  ]  ; 

NavFil 

Bias  psi  = 

x  [  8  ]  ; 

spsi  = 

sin (NavFil 

_psi )  ; 

cpsi  =  cos (NavFil_psi ) ; 


Adt [ 1 ] [ 3 ] 
Adt [ 1 ] [ 5 ] 
Adt  [1]  [6] 
Adt  [2]  [3] 
Adt  [2]  [5] 
Adt  [2]  [6] 
Adt [ 3 ] [ 4 ] 


(-NavFil  Ug*spsi-NavFil  Vg*cpsi)*dt; 
cpsi*dt; 

-spsi*dt; 

(NavFil  Ug*cpsi-NavFil  Vg*spsi)*dt; 

spsi*dt; 

cpsi*dt; 

1 . 0*dt; 


C  [  1  ]  [5] 
C [2  ]  [6] 
C  [ 3 ]  [3] 
C [ 3 ] [8] 


1.0; 

1.0; 

1.0; 

1.0;  /*  120303  Mod  to 


Tests,  returned  to  1.0  111904  */ 


eliminate 


Bias 


learning  for  Compass 


C 

[4] 

[4] 

II 

I—1 

o 

C 

[4] 

[7] 

=  0.0; 

// 

SPK  061406: 

Force  NavFil  Bias  r  to  0.0 

C 

[5] 

[1] 

II 

I—1 

o 

C 

[6] 

[2] 

o 

\ — 1 

II 

C 

[7] 

[3] 

=  0.0; 

// 

measurement 

7  is  not  necessary  when  using  the  IMU 

psi 
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C[7] [8]  =  0.0;  //  measurement  7  is  not  necessary  when  using  the  IMU 


/*  q [3]  =  q [4 ]  =  0.01,  q [5]  =  q[6]  =  0.1,  q[7]  =  0.0  */ 


/*  Orig  Vais  returned  to  original  111904  -  ajh 

q [ 1 ]  =  0.0; 
q [ 2 ]  =  0.0; 
q [ 3 ]  =  0.001; 
q [ 4 ]  =  0.1; 
q [ 5 ]  =  0.01; 
q [ 6 ]  =  0.01; 
q [ 7 ]  =  0.0000001; 
q [ 8 ]  =  0.000001;  */ 

/*  New  Vais  */ 


q[l]  =  0.0;  /*  variance  on  LatDeg  */ 

q [ 2 ]  =  0.0;  /*  variance  on  LongDeg  */ 

q [ 3 ]  =  0.001;  /*  variance  on  psi,  (Radians) A2  returned  111904 

ajh  */ 

q [ 4 ]  =  0.1;  /*  variance  on  r,  rad/sec) A2  */ 

q[5]  =  0.01;  /*  variance  on  Ug,  (m/sec) A2  */ 

q [ 6 ]  =  0.01;  /*  variance  on  Vg, (m/sec) A2  */ 


q[7]  =  0.0000001;  /*  variance  on  NavFil_Bias_r ,  (Rad/sec)  */ 
q [ 8 ]  =  0.0;  /*  variance  on  NavFil  Bias  psi  (Radians)  */ 

/*  Create  Diagonal  Q  Matrix  */ 

f or ( i=l ; i<=8 ; ++i ) 

{ 

Q [ i ]  [i]  =  q [ i ]  ; 

} 

/*  Orig  Vais 

//  nu  [  1 ]  =  0.01; 

//  nu  [2 ]  =  0.01; 

//  nu  [ 3 ]  =  0.1; 

//  nu  [ 4 ]  =  0.001; 

//  nu  [ 5 ]  =  1.0; 

//  nu  [ 6]  =  1.0;  */ 

/*  New  Vais  */ 

//  SPK  081606:  Default  values 

nu [ 1 ]  =  0.001; 

nu [2 ]  =  0.0001; 

nu [ 3 ]  =  0.001; 

nu [ 4 ]  =  0.001; 

nu [ 5 ]  =  1.0; 
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nu [ 6]  =  1.0; 
nu [ 7 ]  =  1.0; 

//  SPK  081606:  Set  elements  of  R  Matrix  based  on  NSv  (as  verified  by 
SRV  08/2006) 

if  (NSv  <=  3) 

{ 

nu [ 5 ] =1 . 0 ; 

nu [6] =1 . 0; 

} 

else  if  (NSv  ==  4) 

{ 

nu [ 5 ] =0 . 1 ; 
nu [6] =0 . 1; 

} 

else  if  (NSv  >=  5) 

{ 

nu [5] =0 . 01; 
nu [6] =0 . 01; 


/*  Create  Diagonal  R  Matrix  */ 
for ( i=l ; i<=7 ; ++i ) 

{ 

R [ i ] [ i ]  =  nu [ i ] ; 


/ 

*  R  [ 

i]  [i] 

} 

p 

[1] 

=  0 

.01; 

p 

[2] 

=  0 

.01; 

p 

[3] 

=  0 

.01; 

p 

[4] 

=  0 

.01; 

p 

[5] 

=  0 

.01; 

p 

[6] 

=  0 

.01; 

p 

[7] 

=  0 

.01; 

p 

[8] 

=  0 

.01; 

/*  Create  Diagonal  P  Matrix  */ 
f or (i=l ; i<=8 ; ++i) 

{ 

P[i] [i]  =  p[i] ; 

} 


//  SPK  060106:  Write  initialized  values  to  first  line  of  NavD  file 
time_of_day  =  time (NULL) ; 
gmtime_r ( &time_of_day, Stmbuf) ; 
ftime ( Stimebuf ) ; 

Month  =  tmbuf.tm  mon+1; 

Day  =  tmbuf.tm  mday; 

Year  =  tmbuf.tm  year+1900; 

Hour  =  tmbuf.tm  hour; 

Minute  =  tmbuf.tm  min; 

Second  =  ((double)  (tmbuf.tm  sec+timebuf .millitm/1000 . 0) ) ; 
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fprintf (Outfp, "%d  %d 
%f  %d  %d  %d  %d  %f  %f  %f 
%d  %f  %f  %f  %f  %d  %f  %f 
%f  %f  %f  %f \n" , 

Nav  Id, 

Hour, 

Minute, 

Second, 

MP_Id, 

KG_q, 

MP_q, 

MP_r , 

RDI_Id, 

RDI_Ug, 

RDI_Vg, 

RDI_Wg, 

RDI_Uf , 

RDI_Vf , 

RDI_Wf , 

0.0, //RDI  AltRawComp,  //SPK:  use  0.0  because  AltRawComp  not 
initialized  yet 

RDI  AltRaw,//RDI  AltEst,  //SPK:  use  AltRaw  because  AltEst  not 
initialized  yet 

0 . 0, //RDI^AltDot,  //SPK:  use  0.0  because  AltDot  not 

initialized  yet 

RDI  Heading, 

GPS_Id, 

GPS_Signal , 

Dif  f , 

NSv, 

ToC, 

LatDeg, 

LongDeg, 

TtTc, 

DepthRaw, 

DepthEst, 

DepthDot, 

MP_phi , 

MP_theta, 
v_ls , 
v_rs , 

NavFil_X, 

NavFil_Y, 

NavFil  psi, 

NavFil  r, 

NavFil  Ug, 

NavFil  Vg, 

NavFil  Bias  psi, 

NavFil  Bias  r. 

Analog  Id, 

BatteryVoltageRaw, 

BatteryVoltage, 

GPS_X, 

GPS_Y, 

HMR_Id, 


%d  %f  %d  %f 

%f  %f  %f  %f 
%f  %f  %f  %f 


S--F  S-f  S-H  S--F 

ol  ol  oLl  ol 

2-  -F  2-  f  9"F  2--F 

ol  ol  ol  ol 

2-  -f  2-  f  2-  -F  2--F 

ol  ol  ol  ol 


2-  -F  9"F  9"F  9"F 

ol  ol  ol  ol 

2-  -F  2-  -F  S--F  2-  -F 
ol  ol  ol  ol 

2-  -F  2-  f  2-  -F  2--F 

ol  ol  ol  ol 


2-  -F  2-  f-  2-  -F  2--F 

ol  ol  ol  ol 

2-  -F  2-  -F  2-  -F  2--F 

ol  ol  ol  ol 

2-  -f  2-  f  2-  -F  2--F 

ol  ol  ol  ol 
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HMR  HeadingRaw, 

HMR  Heading, 

HMR_Pitch, 

HMR_Ro 11, 

KG_r , 
psi, 

Pdop, 

Hdop, 

Vdop, 

Tdop, 

MP_XAccel, 

MP_YAccel, 

P [ 1 ] [1],  //  SPK:  initial  diagonal  elements  of  P  matrix 

P [ 2 ]  [2],  //  SPK:  initial  diagonal  elements  of  P  matrix 

P[3] [3],  //  SPK:  initial  diagonal  elements  of  P  matrix 

P [ 4 ] [4],  //  SPK:  initial  diagonal  elements  of  P  matrix 

P [ 5 ]  [5],  //  SPK:  initial  diagonal  elements  of  P  matrix 

P [ 6 ] [6],  //  SPK:  initial  diagonal  elements  of  P  matrix 

P [ 7 ] [7],  //  SPK:  initial  diagonal  elements  of  P  matrix 

P[8] [8],  //  SPK:  initial  diagonal  elements  of  P  matrix 

P [ 1 ] [8],  //  SPK:  initial  1,8  element  of  P  matrix 
P[2] [8]);//  SPK:  initial  2,8  element  of  P  matrix 
//  SPK  060106:  End  write  of  initialized  values  to  first  line  of  NavD 
file 


//  SPK  011106:  Create  channel  and  connection  for  timer  pulse 
timerChld  =  ChannelCreate ( 0 )  ; 

timerConld  =  ConnectAttach (ND_L0CAL_N0DE,  0,  timerChld, 

_NTO_S I DE_CHANNEL ,  0); 

/*  Get  a  Proxy  for  the  Timer  to  Kick  */ 

//  SPK  011106:  No  longer  using  the  old  code  (or  mig4nto  version) 

/* 

LoopTimerProxy  =  qnx  proxy  attach (  0,  0,  0,  -1  ); 

if (LoopTimerProxy  ==  -1) 

{ 

printf (  "Unable  to  Attach  Proxy."  ); 
return; 

} 


/*  Attach  to  the  Timer  */ 

//  SPK  011106:  Initialize  pulse  event  using  Neutrino  macro 
//  old:  event. sigev  signo  =  -LoopTimerProxy; 

SIGEV_PULSE_INIT (  Sevent,  timerConld,  getprio(O),  Nav_PULSE_CODE,  0 


//  SPK  010306:  Neutrino  version  returns  timer  ID  in  third  paramete 
//  old:  LoopTimerld  =  timer  create (CLOCK  REALTIME, Sevent) ; 
timer_create  (CLOCK_REALTIME,  Sevent,  SLoopTimerld) ; 
if (LoopTimerld  ==  -1) 

{ 

printf (  "Unable  to  Attach  Timer."  ); 
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} 


return; 


/* 

*  1  nano-seconds  before  initial  firing, 

*  1.0/Hz  second  repetitive  timer  afterwards. 

*/ 

LoopTimer.it  value. tv  sec  =  OL; 

LoopTimer.it  value. tv  nsec  =  1L; 

LoopTimer.it  interval. tv  sec  =  OL; 

/*  Convert  Hz  into  NanoSecond  Period  */ 
LoopTimer.it  interval. tv  nsec  =  (int)  (1 . 0/ ( (float) 
Hz) *pow (10.0,9.0) ) ; 

timer_settime (LoopTimerld, 0, SLoopTimer, NULL) ; 


while (TRUE) 

{ 

/*  Wait  for  the  Proxy  */ 

//  SPK  011106:  No  longer  using  old  code  (or  mig4nto  version) 

/ / old:  Receive (LoopTimerProxy, 0,0) ; 

timerRcvId  =  MsgReceive (timerChld,  SpulseMsg,  sizeof (pulseMsg) , 

NULL)  ; 


// 

received 

// 

from  our 
/* 


SPK  011106:  Can  check  if  timerRcvId  ==  0  to  verify  a  pulse  was 
and 

can  check  if  pulseMsg . code  ==  Nav  PULSE^CODE  to  verify  it's 
timer 

Do  Work  */ 


expAdt88 (&Adt, &phi, 8) ; 

spsi  =  sin(*(x+3)); 
cpsi  =  cos(*(x+3)); 


* 

* 

* 

* 

* 

* 

* 

* 


(x  barl+1) 
(x  barl+2) 
(x_barl+3 ) 
(x_barl+4) 
(x_barl+5 ) 
(x_barl+6) 
(x  barl+7) 
(x  barl+8) 


=  * (x+1 )  + 

=  * (x+2 )  + 

=  * (x+3)  + 
=  * (x+4 )  ; 

=  * (x+5 )  ; 

=  * (x+6) ; 

=  * (x+7)  ; 

=  * (x+8)  ; 


( * (x+5 ) *cpsi 
(* (x+5) *spsi 
* (x+4) *dt; 


* (x+6) *spsi) *dt; 
* (x+6) *cpsi) *dt; 


transpose ( &phi , &phi_t ,8,8) ; 

AdotB (&P, &phi_t, &Pphi_t, 8,8,8) ; 

AdotB (&phi, &Pphi_t, &phiPphi_t,  8,8,8) ; 
ApmB ( &phiPphi_t, &Q,  &M,  8,8,  '  +  '  )  ; 


* (yhat+1 ) 
* (yhat+2 ) 
* (yhat+3) 
* (yhat+4 ) 
* (yhat+5) 


* (x_barl+5) ; 

* (x_barl+6) ; 

* (x  barl+3 )  + 
* (x  barl+4 )  + 

* (x  barl  +  1 )  ; 


* (x_barl  +  8)  ; 
* (x  barl+7 ) ; 
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* (yhat+6)  =  *(x_barl+2); 

*(yhat+7)  =  * (x_barl+3)  +  *(x_barl+8); 

for ( j— 1; j<=7;  ++j  ) 

{ 

* (ym  prev+ j )  =  *(ym+j); 

} 

ReadRDIShm (RDI_Shmid, &RDI_Id, &RDI_Ug, &RDI_Vg, &RDI_Wg, 

&RDI_Uf , &RDI_Vf , &RDI_Wf , 

&RDI  AltRaw, &RDI  Heading, 

&RDI  Error) ; 

/*  Check  for  HungUp  Process  */ 
if (RDI_Id  ==  LastRDI_Id) 

{ 

++RDI  IdDupCnt; 

} 

else 

{ 

RDI_IdDupCnt  =  0; 

} 

if (RDI_IdDupCnt  >=  MaxRDI_IdDupCnt) 

{ 

fprintf (AbortLogfp, "MaxRDI_IdDupCount  =  %d  @  RDI_Id  =  %d\n", 
RDI_IdDupCnt, RDI_Id) ; 
fflush (AbortLogfp)  ; 

WriteTacticalMessage  (NetFlag__Shmid,  "ABORT")  ; 

ABORT^FLAG  =  TRUE; 

} 

LastRDI_Id  =  RDI_Id; 

/*  psi  =  RDI  Heading*DegRad;  */ 

ReadHMRShm (HMR_Shmid, &HMR_Id, &HMR_HeadingRaw, &HMR_Heading, 

&HMR_Pitch, &HMR_Roll, &HMR_Error ) ; 

/*  Check  for  HungUp  Process  */ 
if (HMR_Id  ==  LastHMR_Id) 

{ 

++HMR_IdDupCnt; 

} 

else 

{ 

HMR_IdDupCnt  =  0; 

} 

if (HMR_IdDupCnt  >=  MaxHMR_IdDupCnt) 

{ 

fprintf (AbortLogfp, "MaxHMR_IdDupCount  =  %d  @  HMR_Id  =  %d\n", 
HMR_IdDupCnt, HMR_Id) ; 
fflush (AbortLogfp) ; 

WriteTacticalMessage (NetFlag_Shmid, "ABORT") ; 

ABORT_FLAG  =  TRUE; 

} 


105 


LastHMR_Id  =  HMR_Id; 

/*********  This  is  the  New  psi  for  the  EKF  *******/ 

//  SPK  09/01/05 

//psi  =  HMR  Heading*DegRad; 

/*  psi  =  RDI  Heading*DegRad;  */ 


//  SPK  09/01/05 

/*  Integrate  KG  r  to  obtain  KG  psi  */ 

//KG_psi  =  KG_psi  +  dt*KG_r; 

/*  Added  to  eliminate  KG  problems  10  Sep  02  */ 
/*  KG_psi  =  psi;  */ 


ReadMotPakShm (MPShmid, &MP_Id, &MP_phi, &MP_theta, &MP_psi, 

&KG  q, &MP  q, &MP  r, 

&MP_XAccel, &MP_YAccel, &KG_r , 

SBatteryVoltageRaw, SBatteryVoltage, &MP  Error)  ; 

//  SPK  09/01/05 
psi  =  MP  psi; 

/*  Bias  KG_r  */ 

/*  Perform  bias  correction  in  MotPakf.c  instead  --  SPK  3/22/04 
KG_r  =  KG_r  -  1.219e-4; 

*/ 

/*  Check  for  HungUp  Process  */ 
if(MP_Id  ==  LastMP_Id) 

{ 

++MP_IdDupCnt; 

} 

else 

{ 

MP^IdDupCnt  =  0; 

} 

if (MP^IdDupCnt  >=  MaxMP^IdDupCnt) 

{ 

fprintf (AbortLogfp, "MaxMP^IdDupCount  =  %d  @  MP_Id  =  %d\n", 
MP_IdDupCnt,MP_Id) ; 
fflush (AbortLogfp) ; 

WriteTacticalMessage (NetFlag_Shmid, "ABORT") ; 

ABORT_FLAG  =  TRUE; 

} 

LastMP  Id  =  MP  Id; 
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ReadGPSShm (GPS_Shmid, &GPS_Id, &GPS_Signal, &Diff, &NSv, &ToC, &Lat, SLatDeg, 

&Long, SLongDeg, &SbA, &TtTc, &SoG, &Vv, 

&Pdop, &Hdop, &Vdop, &Tdop, &GPS_Error) ; 

//  SPK  081606:  Set  elements  of  R  Matrix  based  on  NSv  (as  verified  by 
SRV  08/2006) 

if  (NSv  <=  3) 

{ 

nu[5]=1.0; 
nu [6] =1 . 0; 

} 

else  if  (NSv  ==  4) 

{ 

nu [ 5 ] =0 . 1 ; 
nu [6] =0 . 1; 

} 

else  if  (NSv  >=  5) 

{ 

nu [5] =0 . 01; 
nu [6] =0 . 01; 


/*  Update  Diagonal  R  Matrix  */ 
for ( i=l ; i<=7 ; ++i ) 

{ 

R  [  i ]  [ i ]  =  nu [ i ] ; 

/*  R[i] [i]  =  nu [ i ] ;  */ 

} 

/*  Check  for  HungUp  Process  */ 
if (GPS_Id  ==  LastGPS_Id) 

{ 

++GPS_IdDupCnt ; 

} 

else 

{ 

GPS^IdDupCnt  =  0; 

} 

if (GPS_IdDupCnt  >=  MaxGPS_IdDupCnt) 

{ 

fprintf (AbortLogfp, "MaxGPS_IdDupCount  =  %d  @  GPS_Id  =  %d\n", 
GPS_IdDupCnt, GPS_Id) ; 
fflush (AbortLogfp) ; 

WriteTacticalMessage (NetFlag_Shmid, "ABORT") ; 

ABORT_FLAG  =  TRUE; 

} 

LastGPS_Id  =  GPS_Id; 

if ( ! GPS  Signal ) 

{ 

/*  We  Don't  have  a  GPS  Signal,  Zero  GPS_X  and  GPS_Y  */ 

GPS_X  =  0.0; 

GPS_Y  =  0.0; 

} 
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else 

{ 

GPS_X  =  Coefl* (LatDeg  -  LatDegO) ; 

GPS_Y  =  Coefl* (LongDeg  -  LongDegO) *cos (DegRad*LatDeg) ; 

} 


ReadAnalogShm (Analog_Shmid, &Analog_Id, SDepthRaw, SDepthEst, SDepthDot, 

SBowLeakVoltage, SMidLeakVoltage, SSternLeakVoltage, 

&v  ls,&v  rs, Sv  bvt, &v  svt, &v  bit, &v_slt, SAnalog  Error) ; 

/*  Check  for  HungUp  Process  */ 
if (Analog  Id  ==  LastAnalog  Id) 

{ 

++Analog  IdDupCnt; 

} 

else 

{ 

Analog  IdDupCnt  =  0; 

} 

if (Analog  IdDupCnt  >=  MaxAnalog  IdDupCnt) 

{ 

fprintf (AbortLogfp, "MaxAnalog_IdDupCount  =  %d  @  Analog_Id  = 

%d\n" , 

Analog  IdDupCnt, Analog  Id); 
fflush (AbortLogfp) ; 

WriteTacticalMessage (NetFlag_Shmid, "ABORT") ; 

ABORT_FLAG  =  TRUE; 

} 

LastAnalog  Id  =  Analog  Id; 

/*  Assign  the  Measurements  */ 

* ( ym+ 1 )  =  RDIJJg; 

//  SPK  09/01/05 

//*(ym+2)  =  RDI  Vg-1.0*KG  r;  /*  This  Takes  into  Account  the  RDI 
Offset  */ 

* (ym+2 )  =  RDI_Vg-l . 0*MP_r; 

* (ym+3)  =  psi; 

//  SPK  09/01/05 

//* (ym+4 )  =  KG_r;  /*  Was  MP_r  */ 

*(ym+4)  =  MP  r; 

* (ym+5)  =  GPS_X; 

* (ym+6)  =  GPS_Y; 

//  SPK  09/01/05 

//*(ym+7)  =  KG  psi;  /*  This  is  a  Virtual  Heading  Ref.  */ 

* (ym+7)  =  psi; 

for ( j=l; j<=7; ++j ) 

108 


res [ j ]  [ 1 ]  =  * ( ym+  j )  -  *(yhat+j); 

} 

/*  To  Kill  Spikes  from  RDI  */ 

/*  Ug  &  Vg  */ 
for ( k=l ; k<=2 ; ++k) 

{ 

if (  (* (ym+k)  ==  * (ym  prev+k) )  | |  (fabs (* (ym+k) )  >5.0) 

II  (* (ym+k)  ==0.0)  ) 

{ 

for ( j  =1 ; j  <=8 ; ++j ) 

{ 

C [ k] [j]  =  0.0; 

} 

} 

} 

/*  Check  Psi  &  r  */ 
for ( k=3 ; k<=4 ; ++k) 

{ 

if ( fabs (  * (ym+k)  -  * (ym  prev+k)  )  <  0.000001) 

{ 

for ( j=l; j<=8; ++j ) 

{ 

C [ k] [j]  =  0.0; 

} 

} 

} 

/*  Check  X  &  Y  */ 
for (k=5; k<=6; ++k) 

{ 

if (  fabs (  * (ym+k)  -  * (ym  prev+k)  )  <  0.000001  ) 

{ 

for ( j=l; j<=8; ++j ) 

{ 

C [ k] [j]  =  0.0; 

} 

} 

} 

if ( ! GPS  Signal ) 

{ 

/*  We  Don't  a  GPS  Signal,  Zero  the  Rows  */ 
for (k=5; k<=6; ++k) 

{ 

for ( j=l; j<=8; ++j ) 

{ 

C [ k] [j]  =  0.0; 

} 

} 
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transpose (&C, &C_t, 7,8) ; 

AdotB ( &M, &C_t , &MC_t ,8,8,7); 

AdotB (&C, &MC_t , &CMC_t, 7,  8,7); 

ApmB (&CMC_t, &R, &CMCR,  7,7,  '  +  '  )  ; 
inv ( &CMCR, &CMCRI ,  7 )  ; 

AdotB (&MC_t, &CMCRI , &L, 8, 7, 7) ; 

AdotB (&L, &C, &LC, 8, 7, 8)  ; 

ApmB  (&I8, &LC, &ImLC, 8,8,  '-'); 

AdotB ( SlmLC, &M, &P, 8, 8, 8) ; 

//  SPK  11/21/05:  store  diagonal  elements  of  P  matrix 
for  (ip  -1  ;  ip<=8;  ip++) 

{ 

P_diag_out [ ip]  =  P[ip] [ip]; 

} 

//  SPK  04/17/06:  store  elements  1,8  and  2,8  of  P  matrix 
P_l_8_out  =  P[l] [8] ; 

P_2_8_out  =  P[2] [8]; 

AdotB ( &L, Sres,  SLres,  8,7,1)  ; 
for ( j  =1 ; j  <=8 ; ++j ) 

{ 

* (x+j )  =  *(x_barl+j)  +  Lres[j][l]; 

} 

//  SPK  052406:  Force  NavFil  Bias  r  to  0.0 

//  SPK  061406:  Undo  this  change  and  do  it  by  zeroing  the  C-Matrix 
elements 

//*  (x+7 )  =  0.0; 

spsi  =  sin(*(x+3)); 
cpsi  =  cos(*(x+3)); 


Adt  [ 

1]  [ 

3] 

=  ( 

- (* (x+5) *spsi) - (* (x+6) ) *cpsi  ) *dt; 

Adt  [ 

1]  [ 

5] 

=  cpsi*dt; 

Adt  [ 

1]  [ 

6] 

=  -spsi*dt; 

Adt  [ 

2]  [ 

3] 

=  ( 

* (x+5) *cpsi  -  (* (x+6) ) *spsi) *dt; 

Adt  [ 

2]  [ 

5] 

=  spsi*dt; 

Adt  [ 

2]  [ 

6] 

=  cpsi*dt; 

Adt  [ 

3]  [ 

4] 

=  1. 

0*dt; 

C  [  1  ] 

[5] 

= 

o 

\ — 1 

c  [2] 

[6] 

= 

o 

\ — 1 

C  [3] 

[3] 

= 

o 

\ — 1 

C  [3] 

[8] 

= 

o 

\ — 1 

/*  Turn  on  Bias  learning  090105  ajh  */ 

C  [  4  ] 

[4] 

= 

o 

\ — 1 

C  [  4  ] 

[7] 

= 

o 

o 

//  SPK  061406:  Force  NavFil  Bias  r  to  0 

C  [5] 

[1] 

= 

o 

\ — 1 
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c [6] [2]  =  1.0; 

//  SPK  09/01/05 
//C  [7]  [3]  =  1.0; 

C [ 7 ] [3]  =  0.0; 

C [ 7 ] [8]  =  0.0;  /*  Mod  to  turn  off  Bias  learning  050405  ajh  */ 

NavFil_X  =  *  (x+1 ) ; 

NavFil_Y  =  *(x+2); 

NavFil  psi  =  *(x+3); 

NavFil  r  =  *(x+4); 

NavFil  Ug  =  *(x+5); 

NavFil  Vg  =  * (x+6) ; 

NavFil  Bias  r  =  *(x+7); 

NavFil  Bias  psi  =  *(x+8); 

/*  Compensate  for  Pitch  &  Roll  */ 

PrevRDI  AltRawComp  =  RDI  AltRawComp; 

RDI_AltRawComp  =  RDI_AltRaw*cos (MP_phi) *cos (MP_theta) ; 

/*  Check  for  Sequential  Zeros  */ 
if (RDI  AltRawComp  ==  0.0) 

{ 

++AltZeroCount ; 
if (AltZeroCount<24 ) 

{ 

SKI P_KALMAN  =  TRUE; 

RDI_AltDot  =  0.0; 

} 

else 

{ 

/*  Too  many  Seqential  Holidays  */ 

/*  KALMAN_ABORT_FLAG  =  TRUE;  */ 

SKI P_KALMAN  =  TRUE; 

} 

} 

else 

{ 

AltZeroCount  =  0; 

if (PrevRDI  AltRawComp  ==  0.0) 

{ 

SKI P_KALMAN  =  FALSE; 

KALMAN^INIT  =  TRUE; 

} 


/*  AltEst  and  Alt  dot  are  Filtered  Outputs  from  the  Kalman  Filter 

*/ 

if ( ! SKI P_KALMAN ) 

{ 

AltitudeKalman (RDI_AltRawComp, &RDI_AltEst, &RDI_AltDot, &KALMAN_ABORT_FLA 
G)  ; 

} 
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if (KALMAN_ABORT_FLAG  ==  TRUE) 

{ 

if (ABORT^FLAG  ==  FALSE) 

{ 

WriteTacticalMessage (NetFlag_Shmid, "ABORT") ; 
fprintf (AbortLogfp, 

"Abort  Due  to  Unstable  Altitude  Kalman  Filterin''); 
fprintf (AbortLogfp, "RDI^AltRaw  =  %f \n" , RDI_AltRaw) ; 
fprintf (AbortLogfp, "RDI^AltEst  =  %f \n" , RDI_AltEst) ; 
fflush (AbortLogfp)  ; 

ABORT^FLAG  =  TRUE; 

} 


if ( (DepthRaw  >  MaxDepth)  | |  (DepthEst  >  MaxDepth) ) 

{ 

if (ABORT_FLAG  ==  FALSE) 

{ 

WriteTacticalMessage (NetFlag_Shmid,  "ABORT")  ; 
fprintf (AbortLogfp, "Abort  Due  to  Exceeding  MaxDepth  = 
%f\n" , MaxDepth) ; 

fprintf (AbortLogfp, "DepthRaw  =  %f\n" , DepthRaw) ; 
fprintf (AbortLogfp,  "DepthEst  =  %f\n" , DepthEst) ; 
fflush (AbortLogfp)  ; 

ABORT_FLAG  =  TRUE; 

} 


if (BatteryVoltage  <  MinBatteryVoltage) 

{ 

if (ABORT_FLAG  ==  FALSE) 

{ 

WriteTacticalMessage (NetFlag_Shmid,  "ABORT")  ; 
fprintf (AbortLogfp, 

"Abort  Since  Below  Minimum  Battery  Voltage  =  %f\n", 
MinBatteryVoltage) ; 

fprintf (AbortLogfp, "BatteryVoltage  =  %f\n" , BatteryVoltage) 
fflush (AbortLogfp) ; 

ABORT^FLAG  =  TRUE; 

} 

} 

if (BowLeakVoltage  >  MaxLeakVoltage) 

{ 

if (ABORT_FLAG  ==  FALSE) 

{ 

WriteTacticalMessage (NetFlag_Shmid, "ABORT") ; 
fprintf (AbortLogfp, "Abort  Due  to  Leak  in  Bow 
Compartment\n" ) ; 

fprintf (AbortLogfp, "BowLeakVoltage  =  %f\n" , BowLeakVoltage) 
fflush (AbortLogfp) ; 

ABORT_FLAG  =  TRUE; 

} 

} 
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if (MidLeakVoltage  >  MaxLeakVoltage) 

{ 

if (ABORT_FLAG  ==  FALSE) 

{ 

WriteTacticalMessage (NetFlag_Shmid, "ABORT") ; 
fprintf (AbortLogfp, "Abort  Due  to  Leak  in  Mid 
Compartment\n" ) ; 

fprintf (AbortLogfp, "MidLeakVoltage  =  %f\n" , MidLeakVoltage) 
fflush (AbortLogfp) ; 

ABORT_FLAG  =  TRUE; 

} 

} 

if ( SternLeakVoltage  >  MaxLeakVoltage) 

{ 

if (ABORT_FLAG  ==  FALSE) 

{ 

WriteTacticalMessage (NetFlag_Shmid, "ABORT") ; 
fprintf (AbortLogfp, "Abort  Due  to  Leak  in  Stern 
Compartment\n" ) ; 

fprintf (AbortLogfp, "SternLeakVoltage  = 

%f \n" , SternLeakVoltage)  ; 

fflush (AbortLogfp)  ; 

ABORT_FLAG  =  TRUE; 

} 

} 


/*  After  setting  Time  using  date  Execute 
rtc  -s  -1  hw 


*/ 


time_of_day  =  time (NULL); 

//  SPK  010306:  Neutrino  uses  gmtime  r()  instead  of  gmtimeO 
//  old:  _gmtime ( &time_of_day, Stmbuf ) ; 
gmtime_r ( &time_of_day, Stmbuf) ; 
ftime ( Stimebuf )  ; 

Month  =  tmbuf.tm  mon+1; 

Day  =  tmbuf.tm  mday; 

Year  =  tmbuf.tm  year+1900; 

Hour  =  tmbuf.tm  hour; 

Minute  =  tmbuf.tm  min; 

Second  =  ((double)  (tmbuf.tm  sec+timebuf .millitm/1000 . 0) ) ; 

WriteBobShm (Bob  Shmid, Month, Day, Year, Hour, Minute, Second, 

NavFil_X,NavFil_Y, DepthEst, RDI_AltEst, GPS_Signal, 

LatDegO ,  LongDegO , HMR  Heading); 
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/*  Write  Estimates  and  Direct  Measurements  Here  */ 

WriteNavShm (Nav_Shmid, 

Nav  Id, 

Month, Day, Year, Hour, Minute, Second, 
NavFil_X, 

NavFil_Y, 

DepthEst, 

RDI_AltEst, 

MP_phi , 

MP_theta, 

NavFil  psi, 

NavFil  Ug, 

NavFil  Vg, 

DepthDot, 

RDI_AltDot, 

KG_q, 

MP_q, 

NavFil  r, 

NavFil  Bias_psi, 

NavFil  Bias  r, 

Dif  f , 

NSv, 

LatDeg, 

LongDeg, 

TtTc, 
v_ls , 
v_rs , 
v_bvt , 
v_svt, 
v_blt, 
v_slt, 

LatO , 

LongO , 

Nav  Error) ; 


fprintf (Outfp, "%d  %d  %d  %f  %d  %f  %f  %f 

%f  %f  %d  %d  %d  %d  %f  %f  %f  %f  %f  %f  %f  %f  %f 

S--F  9-H  &- -F  S-  -F  9-  -F  9-  -F  9-H  9-  -F  9-  -F  9-  -F  9- -F  9-  -F  9- -F  9-  -F  9- -F 

ol  ol  ol  O  -L  ol  o<J.  O  -L  ol  ol  O  -L  ol  ol  ol  ol 

%f  %f  %f  %f  %f \n" , 

Nav  Id, 

Hour, 

Minute, 

Second, 

MP_Id, 

KG_q, 

MP_q, 

MP_r , 

RDI_Id, 

RDI^Ug, 

RDI_Vg, 

RDI_Wg, 

RDI_Uf , 

RDI_Vf , 

RDI  Wf, 


s-  -f 
^5  -L 

9-  f 

9-  -F 


9-  -F 

9-  -F 

9-  -F 
^5  -L 


5f 

5f 

if 


9-  -F 

9-  -F 
^5  -L 

9-  -F 
^5  -L 


9-  f 

9-  -F 

9-  -F 
^5  -L 


9-  -F 
^5  -L 

9-  -F 
^5  -L 

9-  -F 


9-  -F 

9-  -F 

9-  -F 
^5  -L 
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RDI  AltRawComp, 

RDI_AltEst, 

RDI_AltDot, 

RDI  Heading, 
GPS_Id, 

GPS_Signal, 

Dif  f , 

NSv, 

ToC, 

LatDeg, 

LongDeg, 

TtTc, 

DepthRaw, 

DepthEst, 

DepthDot, 

MP_phi , 

MP_theta, 
v_ls , 
v_rs , 

NavFil_X, 

NavFil_Y, 

NavFil  psi, 

NavFil  r, 

NavFil  Ug, 

NavFil  Vg, 

NavFil  Bias  psi, 
NavFil  Bias_r, 
Analog  Id, 
BatteryVoltageRaw, 
BatteryVoltage, 
GPS_X, 

GPS_Y, 

HMR_Id, 

HMR  HeadingRaw, 

HMR  Heading, 
HMR_Pitch, 

HMR_Ro 1 1 , 

KG_r , 

//  SPK  09/01/05 
/ /KG_psi , 
psi, 

Pdop, 

Hdop, 

Vdop, 

Tdop, 

//  SPK  10/24/05 

MP_XAccel, 

MP_YAccel, 

//  SPK  11/22/05 
P_diag_out [ 1 ] , 
P_diag_out [2 ] , 
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P_diag_out [3] , 

P_diag_out [  4  ]  , 

P_diag_out [5] , 

P_diag_out [ 6] , 

P_diag_out [ 7 ] , 

P_diag_out [ 8 ] , 

//  SPK  04/17/06 
P_l_8_out, 

P_2_8_out) ; 

ReadNavFlagShm (NavFlag_Shmid, &Nav_Kill) ; 
if (Nav  Kill)  break; 

/*  Don't  Stop  Navigator  if  ABORT  */ 

/*if (ABORT_FLAG)  break;  */ 

if (Nav  Id>65534) 

{ 

Nav  Id  =  0; 

} 

++Nav  Id; 

++t_count; 

if (t_count  ==  Hz)  t_count  =  0; 
t  =  (double)  (t  count*dt) ; 


/*  Clear  Pending  Proxies  */ 

//  SPK  010906:  Neutrino  equivalent  is  MsgReceive,  preceeded 
//  immediately  by  special  timer  code;  but  this  requires 
//  a  channel  ID  vice  a  process  ID;  for  now,  just  use 
/ /  qnx_proxy_detach ( ) 

//  old:  while (Creceive (LoopTimerProxy, 0, 0)  ==  LoopTimerProxy) 
//  SPK  011106:  No  longer  using  old  code  (or  mig4nto  version) 
//replace:  qnx  proxy  detach (LoopTimerProxy) ; 

ConnectDetach (  timerConld  ); 

/*  Get  Rid  of  the  Timer  */ 
timer_delete (LoopTimerld) ; 

CloseNavDataFile ( ) ; 
fclose (AbortLogfp)  ; 

CloseNavShm (Nav_Shmid)  ; 

CloseNavFlagShm (NavFlag_Shmid) ; 

CloseNetFlagShm (NetFlag_Shmid) ; 

CloseRDIShm (RDI_Shmid) ; 

CloseMotPakShm (MP  Shmid) ; 

CloseGPSShm (GPS_Shmid)  ; 

CloseAnalogShm (Analog_Shmid) ; 

CloseBobShm (Bob_Shmid)  ; 

CloseHMRShm (HMR  Shmid) ; 
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